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SUMMARY 


Work tasks that require extensive manual labor in hazardous environments, 
(e.g., NASA advanced missions) would derive great benefit from increased use of 
automation technology. This report documents a NASA-sponsored activity to de- 
velop the specific technologies needed for increasing the role of automation in 
such missions and to implement a generic computer simulation capability for 
manipulator systems. 

The computer simulation developed provides the ability to perform kinema- 
tic and dynamic analysis of user-defined manipulators. The user establishes a 
manipulator system model, including arms, load objects and an environment, in 
response to program prompts. The task profile is also specified interactively 
by the user and consists of motion segments utilizing position or rate control 
in joint or end-effector motion coordinates, and such nonmotion steps as GRASP 
(for grasping a load object). 

The kinematic analysis provides positions, velocities, and accelerations 
of all parts of the system for the prescribed motion; it incorporates a robust 
iterative joinc solution algorithm for generality. The dynamic analysis 
procedures include requirements analysis, which calculates the system loads 
for specific motions, and response simulation, which gives the motion 
trajectory resulting from a prescribed set of driving torques or feedback 
control law. 

The analysis results can be displayed as printed tabular output or plots 
of the trajectories of relevant parameters. Animated graphic display is avail- 
able during system creation and analysis to verify the system configuration and 
motion. 

The specific automation technologies investigated include control system 
design, trajectory planning and image processing. A general overview of con- 
trol system design is presented, with special emphasis on adaptive control and 
hybrid control of forces and positions. Computer simulation of these control 
modes is available. 

Three algorithms for generating collision-free paths through cluttered en- 
vironments were implemented and compared. The algorithms include a joint space 
search method, a method in which free space is represented by a collection of 
generalized cones, and an incremental motion procedure in which constraints are 
translated locally into joint coordinates. The benefits and drawbacks of each 
method are discussed. 

The literature on edge detection is reviewed in this report. An edge- 
detection algorithm was implemented, along with a computer simulation of a 
manipulator-mounted camera for vision sensing. 

Appendices to this report describe in detail the programming and use of 
the computer simulation package (available through COSMIC). 


vi 



INTRODUCTION 


Background 


This document reports the results of work performed in Tasks 9 through 20 
of contract NAS1-I6759, Evaluation of Automated Decisionmaking Methodologies 
and Development of an Integrated Robotic System Simulation. It was prepared 
by Martin Marietta Denver Aerospace for the Langley Research Center of the 
National Aeronautics and Space Administration (NASA-LRC) in accordance with the 
contract statement of work. These tasks constitute Phases II and III of a mul- 
tiphase activity addressing the technologies relevant to design and operation 
of advanced manipulator systems. Phase I of this activity concentrated on the 
identification and evaluation of artificial intelligence techniques applicable 
to NASA advanced missions and on developing a framework and mathematical models 
for the computer simulation of manipulator systems. The results were docu- 
mented in 1982 (NASA Contractor Reports 165975, 165976 and 165977). 

This project is motivated by the realization that NASA advanced missions 
require increasing use of automation technology for both economical and per- 
formance reasons. Factors influencing this trend include: 

1) . The cost of supporting man in the hostile space environment is much 

greater than that of supporting an unmanned system; 

2) Human strength, dexterity, reach and precision are too limited for 
some applications; 

3) A well-designed automated system provides more optimal control than a 
human controller; 

4) Many mission tasks are highly repetitive and mundane. Automation 
technology is ideally suited for such applications, while humans tend 
to become bored and make more mistakes; 

5) Automation systems are better suited for round-the-clock operations 
that more fully utilize limited resources on space missions. 

Despite these drawbacks involved in direct human operation or control, 
the adaptability, resourcefulness and problem-solving ability of man are still 
needed for the complex dynamic environment associated with space applications. 
Current robot systems are limited to relatively simple, preprogrammed tasks in 
structured environments and incorporate very little machine intelligence and 
external sensing. To achieve the ambitious goals of the space program in such 
areas as space station and long-life reserviceable spacecraft, it is essential 
to reduce direct human control of the robotic systems. This reduction can most 
naturally occur over a four-phase development process. 
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The first phase is to develop the required system with man in the loop to 
provide control and the problem-solving functions. The second phase of robotic 
system evolution is to extract the man from the primary control loop to assume 
a supervisory role. In this role, the operator will perform the function of 
planning out a sequence of tasks to achieve a specific goal. The robotic sys- 
tem will perform the tasks of trajectory planning, obstacle avoidance, and 
joint control. In the third phase, the individual will be extracted one more 
level. In this phase, the operator will perform the function of establishing 
intermediate goals for the robotic system. The robotic system will perform the 
functions associated with breaking down the specific goals into individual 
tasks to be performed. The final phase of robotic system development will re- 
sult in a fully autonomous robotic system. 

To accomplish these goals requires dramatic improvement in some of the ma- 
nipulator component technologies, especially in the fields of sensing, control 
and artificial intelligence. This contract activity addressed several of these 
issues; implementations were developed for image processing (edge detection), 
intelligent path planning, and advanced control strategies (including adaptive 
control and hybrid force/position control). 

Development of the complex technologies associated with advanced automa- 
tion in a timely and cost-efficient manner requires extensive use of computer 
simulation tools that allow implementations to be evaluated and compared be- 
fore building hardware prototypes. The capabilities that a kinematic simula- 
tion (i.e., one in which motions, not forces, are considered) can provide in- 
clude: 

1) Find and display manipulator dexterity arid workspace. This can be 
used to evaluate kinematic designs, suggest workcell arrangement 
(where feasible), and help design systems for maintenance and repair 
by automation; 

2) Verify and implement path planning, including obstacle avoidance and 
singularity detection; 

3) Evaluate improvement in system operation from some types of sensors 
such as proximity sensors or moving video cameras; 

4) Determine potential speed of operation; 

5) Training for teleoperator control using the simulation instead of 
hardware, and evaluate the different levels of human interaction in 
the control loop. 

A dynamic simulation also includes the system forces and provides addi- 
tional capabilities such as: 

1) Verification and evaluation of controller designs, especially those 
that incorporate advanced control concepts. For example, adaptive 
control schemes often involve identification of system parameters from 
response information. With a dynamic simulation, the actual values 
for the parameters are specified so the identification scheme can 
readily be verified; 
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2) Verification of system performance, structural integrity, load distri- 
bution and component stress levels; 

3) Testing the use of force-related sensors such as a force/torque wrist 
or gripper force sensors in system operation; 

4) More accurate simulation for teleoperator training, including control 
involving force-feedback or dynamic interactions with the environment. 

A dynamic simulation package for the entire manipulator system forms an indis- 
pensable tool for design and development of automation implementations for NASA 
advanced missions. 


Contract Objectives 


The primary objectives of this contract activity are the implementation 
of an integrated robotic simulation package and development of the technolo- 
gies relevant to operation of advanced manipulator systems. The research per- 
formed and software developed during the contract phases reported here focused 
on the following capabilities: 

1 ) 


2 ) 


Computer simulation of a robot in operation, including system kine- 
raatics and dynamxcs, mteractxve control of program execution by the 
user, and graphic display of the system operation; 


Computer simulation of multisensor grippers; 


3) Control concepts for manipulators incorporating adaptive techniques 
and control of force levels as well as positions; 


4) Trajectory planning for manipulator motions in unstructured environ- 
ments ; 


5) 


Image processing and simulation of a manipulator-mounted video camera. 


Report Organization 


This report consists of a main text that describes the study results, in- 
cluding the technical aspects of the robotic simulation (ROBSIM) package and of 
the automation technologies investigated, along with two appendices that docu- 
ment the computer implementation of ROBSIM. ROBSIM consists of three func- 
tional packages (Fig. 1): 


3 



1 ) 


System definition function - Allows the user to interactively create a 
manipulator system containing manipulators, load objects and an envir- 
onment. This package addresses Task 12 of the contract; 

2) Analysis tools function - Contains requirements analysis and response 
simulation options for investigating the load/motion relationships of 
a manipulator system in operation, and addresses Tasks 14, 15 and 16 
of the contract; 

3) Postprocessing function - Aids in interpreting the analysis results by 
graphic display of results, replay of simulated motion, etc. 



j Figure 1.- ROBSIM functional blocks. 


The following three sections of this document describe the technical ap- 
proach employed in each of these functions. 

The next section after these sections discusses advanced controller con- 
cepts and their evaluation and simulation. It covers work performed in Tasks 
13, 17 and 19 of the contract. The section after this describes trajectory 
planning research performed for Tasks 9 and 18. Three methods for obstacle 
avoidance were implemented and evaluated. Video simulation and image process- 
ing are discussed in the following section. The video simulation module and 
edge detection algorithm address the requirements of Tasks 10 and 11. The work 
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in Task 20, hardware vs software validation, forms the next section. This sec- 
tion was addredded with cooperation from NASA-LRC. Validation efforts were 
carried out on a 2 DOF planar arm at Martin Marietta and a 6 axis PUMA robot at 
NASA-LRC. The final section of this text summarizes the results of this activ- 
ity and describes avenues for further efforts to expand, enhance and utilize 
capabilities developed during the performance of this contract. 

This document has two appendices available through COSMIC.* Appendix A 
is a ROBSIM User's Guide and describes the steps involved in running the pro- 
gram. Appendix B provides the programmer with additional information concern- 
ing program implementation. This appendix and the in-code documentation pro- 
vide sufficient information to allow modification of the program for special- 
purpose applications. 


* Inquiries concerning the program ROBSIM, Appendix A (User's Guide), and 
Appendix B (Programmer's Guide) should be directed to: COSMIC, 112 Barrow 

Hall, University of Georgia, Athens, GA 30601. 
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MANIPULATOR SYSTEM DEFINITION 


This section describes the methods implemented in the ROBSIM package for 
defining a robotic manipulator system. The discussion of the system definition 
is separated into four subsections: 


1) Manipulator arm creation/ modification; 

2) Environment creation/modification; 

3) Load objects creation/modification; 

4) System creation. 

A system is actually composed of one or more of the components listed in items 
1) through 3) above. 


Manipulator arm creation/ modification is used to define the mass and geo- 
metric properties of one robot arm. This includes properties for the base, all 
joint/link pairs, and the end-effector (also called "tool" or "hand"). De- 
tailed geometries may also be defined for each part of the arm. These, how- 
ever, are used for the graphics displays that accompany the ROBSIM framework 
and do not affect arm motions or any of the analyses methods described later. 


Environment creation/modification is used to simulate immovable objects in 
the workspace of the manipulator arm. Currently the usefulness of the environ- 
ment definition is limited to graphic displays and does not affect or hinder 
the manipulator motion in any way. 


Creation/modification of load objects is very similar to that of the en- 
vironment, with the exception that load objects may be moved around the work- 
space by one or more manipulator arms. 


System creation is used to bring together the components into one coherent 
group. A system may contain as little as one manipulator arm or multiple arms, 
a detailed environment, and a group of load objects. Each component is placed 
in the system with respect to a reference or world coordinate system. Table 1 
lists the notations used in this section. 
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TABLE I. - SYSTEM DEFINITION SYMBOLS 


Xi 

local coordinate system for component i 

[Pi] 

coordinate system i-to-world coordinate system 
transformation matrix 

[jpi] 

coordinate system i to j transformation matrix 

li 

vector defining location of origin of local coordinate 
system of component i in world coordinate system 

Li 

vector defining the location of i in world coordinates 

j^ij 

vector from the origin of joint i to origin of joint j in 
the i coordinate system 

jiijj 

vector from joint j origin to link j center of gravity in 
the j coordinate system 


angular displacement of joint j 

Hi] 

inertia matrix of component i 

m i 

mass of component i 

bi 

radius of simple cylinder representation of component i 

1 i 

length of simple cylinder representation of component i 

Eli, E 2i 

endpoints of simple cylinder representation of 
component i 

Note: If i 

or j is a b, this denotes the manipulator base; if an L it 
refers to a load object. 


Arm Creation/Modification 

The method of manipulator arm definition implemented in ROBSIM separates 
components of the arm into one of three categories: 

1) Base; 

2) Joint/link pairs; 

3) End-effector. 
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The base of the arm is defined first, followed by each of the joint/ link pairs, 
and finally the end-effector. Each component of the arm is defined within its 
own local coordinate system, which is referenced back to the world coordinate 
system. 

Base . - The local coordinate system of a manipulator base is denoted X b . 
The location and orientation of this local coordinate system with respect to 
the world system is defined by the user. Rjj is the vector containing the 
world system coordinates of the origin of the X b coordinate system. Orienta- 
tion of X b with respect to the world system is given by the matrix [P b ] . 

The columns of [P b ] contain direction cosines of each axis of X b with the 
world coordinate system axes. This orientation transformation can be used to 
locate any point of interest with respect to the world using the relation 

£i = R b + [P b ] b £i 

where b £^ is the location of the point of interest in the base coordinate 
system. The base itself is modeled as a simple cylinder with the centerline 
along the base coordinate system x-axis. 

In the current version of ROBSIM, the base is stationary once placed in a 
system. For this reason, mass properties are not defined, only geometric pro- 
perties are. 

Joint/link . - The majority of the arm is described as joint/ link pairs. 
Each joint is defined to have a specific location and orientation. However, 
all mass properties of the joint/link pair are associated with the link. The 
local coordinate system for each joint is denoted by where i is the joint 
number. The joints of the arm are numbered consecutively, starting with joint 
1 next to the base. Three types of joints are available for ROBSIM modeling: 

1) Hinge joints; 

2) Swivel joints; 

3) Sliding joints. 

Hinge joints rotate about the joint local y-axis, swivel joints about the 
x-axis, and sliding joints translate along the x-axis. Figure 2 shows each of 
the joint types. The location of each joint j is specified by the vector ih^ij, 
the vector from the previous joint i (or base if j = 1) to the current joint j 
in the X^ coordinate system. To locate this joint in the world coordinate 
system, the transformation matrix [P^] and local joint location vector jhfj are 
multiplied and added to the world system location vector of joint i 

iij J^i + t p i] iilij 
or 

Rj = Ri + [P f] iilij + [Pj]^° j ^ 

for sliding joints, where 0j is the linear displacement for joint j and 
[Pj] is the transformation matrix from the joint j coordinate system to the 
world coordinate system. 
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Joint orientation is also defined with respect to the previous joint with 
the transformation matrix [jPj]. Each column contains the direction co- 
sines of an axis of the Xj coordinate system to the coordinate system 
axes. The transformation matrix [Pj] is then calculated from 

[p j] = [PiHiPjl 

The capability for user-defined effective actuator parameters is included for 
each joint. These parameters are: 

1) Actuator torque constant; 

2) Motor gear ratio; 

3) Actuator amplifier gain; 

4) Back EMF constant; 

5) Motor effective inertia; 

6) Motor winding resistance; 

7) Motor winding inductance; 

8) Coulomb friction coefficient; 

9) Static friction coefficient; 

10) Effective viscous damping. 

The use of these parameters is discussed in detail in the Analysis Capability 
section. The initial joint position 0 j is the displacement of the joint 
measured from its original location and orientation. 

The link accompanying each joint is defined as being placed after the 
joint. For example, link 2 goes from joint 2 to joint 3. The link is defined 
initially as a simple cylinder with its centerline along the joint local 
x-axis. Endpoints of the link are just coordinates along this axis. Figure 3 
shows joint/ link sequencing and locations. ROBSIM puts the center of mass at 
the geometric center of the link. The link center of mass vector jhjj 
then has the coordinates. 

jJljj = ^ E 1 i. » °> 



10 



Multiplying by [Pj] transforms this vector to the world coordinate system. 

As an alternative to this placement of the center of mass, an arbitrary center 
of mass may be defined by user input of the local system coordinates of the de- 
sired center of mass. The algorithm implemented in ROBSIM for calculating the 
link inertia matrix uses the following equations for computing the diagonal 
terms 



Ii 3,3 = Ii 2,2 

The off-diagonal terms are set to zero as there are no cross-products of in- 
ertia for the simple cylinder representation used to model the links. To spe- 
cify a different inertia matrix, the user may input values directly. 

Point masses may be added to each link if desired to create an arbitrary 
mass distribution. The addition of point masses requires that the total mass, 
center of gravity, and inertia matrix of the link be recalculated. The total 
mass is simply the sum of the link mass and all associated point masses 

# of pts 

m ^total = m i + S ““n 
n=l 

The new center of gravity is determined using 

y;i m i Icg i 

— c 8 total = * 

Calculating a new inertia matrix is done using 
[ r i] total " t I i3 + m i [ 1 i ] + “pt^pt] 

[Ii] = (bi.biHE] - b^bT 

[ipt;] = (bpt *^.pt^ ~ kpf.bpf- 

where [E] is the identity matrix and t)j is the vector from the new 
composite eg to the eg of j (link or point mass). 

It should be noted that in addition to the three joint types mentioned earlier, 
"special joints" where the motion between adjacent joints may be constrained 
are also provided for. However, the use of any special joint would require a 
program modification that is not readily available at this time. 

End-effectors . - The end-effector of a manipulator arm is modeled exactly 
as the link of a joint/link pair and includes the same provisions as links do 
for specifying arbitrary mass distributions. 
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Graphics . - ROBSIM has the capability to define detailed shapes for any 
component of the manipulator arm. Each part of the arm may be described as 
one of, or a combination of, any of the following shapes: 


1) 

Cylinder ; 

6) 

Triangular structure; 

2) 

Cone ; 

7) 

Data tablet structure; 

3) 

Rectangular solid; 

8) 

Fillet; 

4) 

Symmetric trapezoid; 

9) 

Nonplanar entity. 

5) 

Nonsymmetric trapezoid; 




These detailed geometries are used only for the Evans and Sutherland graphics 
display. All mass properties and arm motions are based on simple cylinder re- 
presentations of the arm components. Figure 4 shows a detailed graphic repre- 
sentation of a manipulator arm. 


Environment Creation/Modification 


Definition of an environment simulates the workspace or surroundings of a 
manipulator system. Any components considered to be part of the environment 
are completely stationary once placed in the system. Consequently, no mass 
properties are ever defined and the environment is used only for the Evans and 
Sutherland graphic display. There is no effect on manipulator arm motion or 
any of the analysis capabilities. Environment definition is carried out by 
specifying the placement of detailed geometric components in the world coor- 
dinate system. The geometries available are: 


1) 

Cylinder; 

6) 

Triangular structure; 

2) 

Cone; 

7) 

Data tablet structure; 

3) 

Rectangular solid; 

8) 

Fillet; 

4) 

Symmetric trapezoid; 

9) 

Obstacle entity. 

5) 

Nonsymmetric trapezoid; 




To specify the placement of these components, each has its own local coordinate 
system whose x-axis is the centerline of the component. The origin of the lo- 
cal coordinate system is contained in the vector r^. The orientation of each 
component's local coordinate system is defined by a transformation matrix whose 
columns contain the direction cosines of the local x, y, and z axis to the 
world coordinate system axes. 
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Figure 4. 


I 


- Detailed graphics representation of manipulator arm. 
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Load Objects Creation/Modification 


Load objects are similar to the environment in that they are used to sim- 
ulate the workspace of a manipulator. Unlike components of the environment, 
load objects are not stationary but may be moved by a manipulator arm. Similar 
to arm link components, each load object is defined initially as a simple cyl- 
inder with a local coordinate system Xl, and with the centerline of the ob- 
ject along the local x-axis. The location of the origin of the X£ coordinate 
system is defined by the vector r^. The orientation of the local coordinate 
system is again defined by a transformation matrix whose columns are the direc- 
tion cosines of the local x, y and z axes to the world coordinate system axes. 

The center of mass of each load object is determined using the method de- 
scribed for manipulator links. A different center of mass may be specified by 
user definition of its x, y, and z coordinates in the X^ coordinate system. 
Inertia matrix calculations are also the same as for the links of a manipulator 
arm. Point masses may be added to a load object to create an arbitrary mass 
distribution. The location of the point mass is defined by the vector 
T.ty t- . The new mass properties (total mass, center of gravity, and inertia 
matrix) are calculated by the same algorithms as used for the manipulator 
links . 


The preceding paragraphs have described the mass properties for load ob- 
jects. Evans and Sutherland graphic displays are available to portray the load 
objects. This capability allows the description of detailed geometries for 
each object. The detailed geometry definition is done by describing each load 
object as a group of one or more of the following geometry components: 


1) 

Cylinder; 

6) 

Triangular structure; 

2) 

Cone ; 

7) 

Data tablet structure 

3) 

Rectangular solid; 

8) 

Fillet; 

4) 

Symmetric trapezoid; 

9) 

Nonplanar entity. 

5) 

Nonsymmetric trapezoid; 




Each of these components has its own local coordinate system whose location and 
orientation is specified with respect to the load local coordinate system. 
Figure 5 shows the relation of component coordinate systems to the load local 
coordinate system. 
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System Creation 


Creation of a manipulator system brings together all of the items dis- 
cussed up to this point — manipulator arms, an environment, and load objects. 

The system is put together with respect to a reference or world coordinate sys- 
tem. This is the same world coordinate system used previously for item defini- 
tion. Adding an environment to the system places it at the same location and 
orientation as specified during environment creation. 
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Placement of a manipulator arm in the system may be modified from the lo- 
cation and orientation called out during arm creation. The new location of the 
arm base is stored in the same vector R b . Location vectors for each joint 
are also updated. The new base orientation is a concatenation of the matrix 
[Pfc] specified during arm creation and the matrix [bPb']> which specifies 
the new orientation with respect to the old orientation. The transformation 
matrix for the new base orientation with respect to the world is found using 

[P b .J = [P b HbPb'] 

The orientation of each joint in the rest of the manipulator arm is revised 
using the same procedure 

[Pi'] = [PiHbPb'J 

Placement of load objects in the system is identical to manipulator arm 
placement. Each object may be placed in a new location and/or orientation. 
Figure 6 shows an Evans and Sutherland display of a system that includes one 
arm, a table as the environment, and two load objects 
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Figure 6. - Complete system definition. 
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ANALYSIS CAPABILITY 


Introduction 


This section describes the analysis tools implemented in the ROBSIM pack- 
age for investigating load and motion properties of general manipulator sys- 
tems. Three types of analysis can be performed on the user-defined system: 

1) Kinematic analysis; 

2) Requirements analysis; 

3) Response simulation. 


Kinematic analysis involves determining positions and position derivatives 
(motion) of the manipulator links. It is important in its own right for in- 
vestigating arm reachability, workpiece placement, task sequencing, obstacle 
avoidance, tool rate limits, etc; it also forms the basic step in all appropri- 
ate dynamic analysis formulations. 


Requirements analysis and response simulation are the two dynamic analysis 
options available in the ROBSIM package. Requirements analysis, also referred 
to as "inverse dynamics" or "kinematically driven analysis," involves determi- 
ning the operating forces and torques for a prescribed motion state of the ma- 
nipulator. This provides actuator torque and sizing criteria, component stress 
levels, load-handling limits, feedforward compensation values for control, etc. 


For response simulation, or "force-driven analysis," the actuator driving 
torques are specified (possibly in the form of a feedback control law) and the 
resulting motion trajectory of the manipulator is calculated. This option is 
especially useful for evaluating control strategies, task performance, inter- 
actions with the environment, etc. 


The final part of this section describes the algorithms employed in the 
ROBSIM package for modeling end-effectors, in particular the parallel jaw 
gripper developed and in use at the Intelligent Systems Research Laboratory 
(ISRL) at NASA-LRC . 


Some results obtained using these ROBSIM analysis tools are demonstrated 
in subsequent sections of this report. Table II lists the notation used in 
this section. 
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.TABLE II.- NOTATION IN ANALYSIS CAPABILITIES DISCUSSION 



Acceleration of Special Point i 

UJ 

Jacobian Matrix Relating Hand Motion to Joint Motion 

V 

Velocity-Related Acceleration of Special Point i, i.e., Acceleration 
When Joint Accelerations Are Zero 

4 

Vector of Jacobian Relating Motion of Link i to Joint J 

[A) 

Effective Inertia Matrix in Equations of Motion 

K 

amp 

Amplifier Gain in Gripper Drive 

b 

Vector of Position- and Velocity-Related Torques In Equations of 
Motion 

K 

ec 

^emf 

Effective Coulomb Friction Coefficient 
Effective Back-emf Constant for Motor 

c i 

Contact Points between Peg and Gripper, i“l , 2 

K es 

Effective Static Friction Coefficient 

Is, 

Direction of Tangential Velocity of Constrained Point 

K et 

Effective Torque Constant for Motor 

! f ci 

Force at Contact Point c^ 

K 

ev 

Effective Viscous Friction Coefficient 

j f 8 r 

Gripping Force 

[K ] 
sp J 

Matrix of Spring Constants Relating End-Effector Force to Position 

it 

Force at Point i 

Sach 

Coefficient Relating Tachometer Voltage to Gripper Velocity 

f 

Reaction Force, at Constraint, with Magnitude f f or f + - f 

i. 

p 

Length of Peg 

&. 

Acceleration Due to Gravity 

M i 

Mass of Link i 

hi 

Vector from Origin of Link i to the Centroid of That Link* 

"a 

Actuator Gear Ratio 

-1,1+1 

Vector from Origin of Link i to Origin of Link i+1* 

Sr 

’ Vector Normal to Constraint Surface 

i 

Index, Generally 1, 2, N+l Indicating Link i, But Also b-Base, 

L-Load, gr-Gripper, p-End-Ef fector Reference Point, eg -Centroid 
of Link I 

N 

Number of Joints 


Effective Inertia of Motor and Drive 

N+l 

Refers to End-Effector 

I 

gr 

Effective Inertia Associated with Cripping Motion 


Matrix of Direction Cosines Specifying Orientation of ith Coordinate 
System with Respect to World Coordinate System 

1 

i fl i ] 

* „ . » ' ' p rof 

Inertia Matrix of Second Moments of inertia about Centroid for Link i , [i i+1} 

Transformation Matrix from Coordinate System i to the Reference 
Position of Coordinate System i+1 
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TABLE II. - (CON CL) 


: jr 

Vector Locating a Point of Interest in World Coordinate System* 


Angular Acceleration of Body i 



r 

- c *i 

Location of Center of Gravity of Link i in World Coordinate System 

V 

— 1 

Velocity-Related Angular Acceleration of Body i 



r 

-P 

Location of End-Effector Reference Point or End of Peg 

Up] 

Change in an Orientation Matrix 



R 

a 

Motor Armature Resistance 


Change in Position of End-Effector Reference Point 



h 

Vector Locating Origin of ith Reference 


Error in Position of Tool Origin Measured from Current 
Position 

to 

Desired 

q 

Generalised Velocity Coordinate Used in Rate Control 

a* 

Rotation Vector Representing a Change in Orientation 



S 

Generalised Momentum in Joint Coordinates 

A6_ 

Small Displacement of Joints 



S 

Laplace Variable 

❖ 

Rotation Angle Corresponding to Change in Orientation 



ii 

Unit Vector along Axis of Link i. Referenced to World Coordinate System 

1 

Relative Joint Displacements - Used As Generalized Coordinates 

C 

Time 

9 i 

Relative Joint Displacement at Joint i 




Reaction Torque at Joint i 

V 

Gripper Displacement Angle 



lx 

Generalized Impulse in Joint Coordinates 

u 

Coefficient of Friction at Constraint Contact Surface 



u(t) 

Scalar Representing Proportion of Motion Segment Traversed by Time t 

0 

Special Implicit Constraint Function 



V dac 

Control Voltage Driving Gripper Operation 

o 

Signum Function, Equal to* 1 



kl 

Velocity of Reference Point i 

T_ 

Vector of Generalized Torques 



! 

!\; 

j ' 1 


-i 

Angular Velocity of Body i 



h 

Vector Transforming Constraint Friction Force to Joint Coordinates 





j 

j-r 

Vector Transforming Constraint Reaction Force to Joint Coordinates 





'*1 

Coordinate System Associated with Component i 

*A subscript i preceding these vectors indicates that the vector 
relative to the local coordinate system i. 

is 

specified 




Kinematic Analysis 


The kinematic and dynamic analysis tools implemented in ROBSIM are based 
on a rigid-link model of serial, open-loop kinematic chains with one-degree-of- 
freedom joints. This subsection first describes the forward kinematics solu- 
tion. This is, given the vector & of relative joint displacements and their 
first and second time derivatives (0^, 0) for a specified arm geometry, find the 
positions, orientations, velocities and accelerations of all links and points 
of interest in the device. Then methods^ for generating the time trajectories 
of the joint displacements (0(t), 0(t), 0(t)), particularly for providing task- 
oriented operation of the manipulator, are discussed. 

Link positioning . - Determining the position and orientation of each link 
is a fundamental step in the analysis of complex mechanisms such as manipula- 
tors. Figure 7 presents a simple graphical representation of a serial manipu- 
lator arm. As described in the preceding section of this report, a local Car- 
tesian coordinate system X£ is associated with each link of the arm. The 
complete position of the link is specified by the coordinate position of one 
point in the link and the orientation of the link. 



Figure 7.- . , 

Kinematic representation of a serial manipulator. 
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The vector contains the components of the location, in an inertial 
world coordinate system, of the origin of the X£ reference (Fig. 7). The 
orientation of the link is defined by the 3x3 rotation matrix of direction co- 
sines [PjJ. The matrix [Pf] is an orthogonal matrix; each column contains 
a unit vector specifying the direction, in world coordinates, of the correspon- 
ding local reference axis. Although this method for specifying orientation 
contains an overabundance of parameters (nine parameters, of which only three 
are independent), it is convenient for several reasons, including: 

1) The [P£] matrix forms the basis for transforming vectors from one 
coordinate system to another; 

2) It contains a column specifying the direction of the joint axis, which 
is needed for subsequent computations; 

3) It is readily determined uniquely from other orientation specifica- 
t ion s . 

As an example of the coordinate transform, suppose ,*£, specifies the di- 
rection of a vector in terms of the local coordinate system X^. The direc- 
tion of this vector in the world coordinate system is given by 


Recall that the preceding subscript indicates the coordinate reference for the 
vector, with the default value W for the world reference. The full point 
transformation is obtained by adding the vector to the origin of Xi» That 
is, if ,r specifies the local coordinates of a point fixed in link i, then 
the coordinates of this point in the world reference are* 

r = R . + [P.].r 
— — x. I x— 


Note that since [PjJ is orthogonal. 


[P.]" 1 = [P^ 1 

where the T superscript indicates transpose; also, successive transformations 
are produced by matrix multiplication 


W ■ W [ i'i J 


so transformations between arbitrary sets of coordinates are readily obtained. 


A recursive method is employed for finding the link positions; it is com- 
putationally efficient and readily programmed. The orientation matrices for 
the links are successively computed starting with the base and proceeding along 
the serial chain to the free end of the manipulator. Suppose that the rotation 
matrix [P^] for link i is known. The corresponding values for link i+1 are 
then given by 

! t P i+l^ = ^i P i+l ] 


*This transformation is often represented in homogeneous coordinates using 
4x4 displacement matrix [A] (See [Paul 1981a]) 


0 0 0 I 1 


a 
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[A] = 



The orientation matrix between successive links i and i+1 consists 

of two components: (1) a fixed link rotation matrix + ^] that 

defines the transformation from coordinate system i to the reference position 
of system i+1 and is established during system definition, and (2) a joint ro- 
tation matrix [£?!+]_] that varies with the joint displacement for 

rotating joints. Because hinge joints rotate about the local Y-axis and swivel 
joints rotate about the local X-axis, the joint rotation matrices are defined 
by the following 


(Hinge Joint i+1) 


(Swivel Joint i+1) 


i+1) 


The only place in the formulation that trancendental functions are evaluated is 
in setting up these joint rotation matrices. The full recursive relation for 
reference i+1 is computed as 

■ ‘V t± p m> 

Successively applying this relation starting from a known position of the base, 
each link's orientation matrix is obtained. 

Once these rotation matrices are determined, vectors are transformed from 
local to world coordinates and the locations of the coordinate origins are re- 
cursively computed. For rotating joints i+1, the relation is 

= R.^ + (Rotating Joint i+1) 

where 

^±,i+l = tP i^ i^i , i+1 

and is the vector, in local coordinates, from link i to link i+1, 

which is specified during system definition., If joint i+1 is a sliding joint, 
an additional term for the displacement along the joint axis (local x-axis) 
must be added (see Fig. 7) 





c ° se i+i 

0 

sin8 1+ i 

= 

0 

1 

0 


-sin0 . . 1 
l+l 

0 

cos6 i+l 

r 

1 0 


0 


Vl+i) 


i 


0 cos0 
0 sine 


i+1 

i+1 


- sin9 i+i 


cos 6 


i+1 


ti p i + i] 


1 

0 

0 


0 0 
1 0 
0 1 


(Sliding Joint 
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— i+1 -i + ^L,i+1 + ^ P i+1 ] 



(Sliding Joint i+1) 


Again, successive application of these recursive relations yields the location 
of each local coordinate reference. 


Note that once the position of each link's reference Xi is available, 
the world coordinates for any point in the link can be found using the point 
transformation given earlier. For example, i_£ C gi represents the local 
coordinates of the centroid of link i and is established during system defi- 
nition. The instantaneous location of this centroid in terms of the world re-> 
ference is given by 


r = R. + [Pj .r 
,-cg i -a. i i-cg 1 

Link and point velocities . - The velocity of a rigid body is conveniently 
specified as the translational velocity of some point in the body along with 
the angular velocity of the body. A recursive method based on classical rigid- 
body kinematics is employed to calculate the velocities and accelerations of 
the individual manipulator links. The method has received much attention in j 
the recent literature on manipulator dynamics (e.g., see [Orin 1979], [Luh 
1980], [Walker 1982]), and was chosen for its efficiency, simplicity, and ease] 


M" 


Jt 


of programming. 

,..U 

Let represent the angul/ar velocity of link i referenced to the world 
coordinate system. Assuming is known, /the angular velocity ^i+i of the 
next link is readily given as the sum of plus the angular velocity of link 
i+1 relative to link i, ^i+l/i- If joint i+1 is a rotating joint, then this 
relative angular velocity is a vector of magnitude 0^+1 about the axis of 
rotation, i.e., / 


—i+1 / i 9 i+1 ^i+1 (Rotating Joint i+1) 


Here, S j +i is a unit vector along the joint axis and is given in terms of the 
link's orientation as 


§1+1 



(Hinge Joint i+1) 


§ 1+1 



(Swivel or Sliding Joint i+1) 
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Therefore, starting with the specified angular velocity for the base, the angu- 
lar velocity of each link is obtained by successive application of the recur- 
sion relations 


£±+l 


% 


6 i+l ^.+1 


(Rotating Joint i+1) 


= (Sliding Joint i+1) 

Subsequently, the translational velocity of the origin of the 

2.1+1 reference can be represented in terms of the translational and rotation- 
al velocities of link i. If joint i+1 is a rotational joint, the relation is 

V i+1 = V i + to ^ x (R i+1 - R^) (Rotating Joint i+1) 

while an additional term must be included for the sliding joint case 

V.., = V. + a), x (R. , , - R.) + 6,,, S, (Sliding Joint i+1) 

—i+1 — x — x —i+1 — i i+1 —1+1 

Recall that the vector from joint i to joint i+1, which is used in the vector 

crossproduct term in these equations, is given by 

R . , . - R. = h , (Rotating Joint i+1) 

—1+1 —x — i , i+1 


—i+1 ~ — i ~L,i+l + 8 i+l -i+1 


(Sliding Joint i+1) 


In addition, if the velocity V„ of any point P fixed in link i is desired, it 
is then obtained from 


V = V. + u, x ( r 

— p — i — a. — p 



For a specified velocity Vjj of the manipulator base, the application of these 
formulas provides values for the velocities of each link and all points of in- 
terest directly from the position results. 


Link and point accelerations . - In a similar manner, the accelerations of 
the manipulator links can be solved for by recursive application of the second- 
order motion derivatives for rigid bodies. Let r aj. represent the angular ac- 
celeration of link i and a.^ the translational acceleration of the correspon- 
ding reference origin. The acceleration equations are as follows: 


-i+1 + % X ®i+l ^L+i + 2 i+1 (Rotating Joint i+1) 

—i+1 ~ (Sliding Joint i+1) 
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a., = a J + (u. x [ok x (R. , , - R.)] + a, x (R. , , - R.) (Rotating Joint i+1) 

— i+1 — i — i — i —i+1 — i — 1 — l+± — 1 


a ±+1 = a ± + ok x [a^ x (R 1+1 - R ± ) ] + ^ * (£*+! ~ V 


+ 2 ^l x e i+1 ^ +1 + e i+1 ^i +1 


(Sliding Joint i+1) 


Again, the accelerations ajj and cyj of the base are presumed known, and the 
acceleration £p of any point of interest in link i can be readily obtained: 

Ap - Si + “i x [u>. x (rp - R*)] + x (Tp - R.) 

In particular, the acceleration £ C g£ of each link's centroid is calculated 
for use in the subsequent dynamic analysis. 

Note that all vectors used in the preceding equations are expressed in 
terms of the world reference system. Any other coordinate selection is poss- 
ible; the only constraint is that a consistent set of coordinates must be used 
throughout each equation. 

End-effector . motion . - While the kinematics and dynamics of serial manip- 
ulators are most conveniently evaluated in terms of the joint displacements and 
rates, it is often more useful to prescribe the motion of the terminal link 
("end-effector," "tool," or "hand") of the device, especially for task-oriented 
operation. The algorithms employed in the ROBSIM package for transforming end- 
effector motion specifications into joint motion states are described here. 

The inverse positioning problem, that of finding a set of joint displace- 
ments 0 corresponding to a prescribed end-effector position can be exceedingly 
complex* and analytic solution methods are not readily generalizable. However, 
commercial robots generally have special geometries, resulting in much simpler 
positioning solutions (e.g., see [Duffy 1979], [Paul 1981b]). Special routines 
can be programmed for certain geometries or classes of geometries. For gener- 
ality, an iterative positioning routine is implemented in ROBSIM; because it 
involves an extension of the velocity results, it is described after the velo- 
city relations are presented. 


*For example, the solution for a six-degree-of-freedom arm can involve finding 
roots to a polynomial of up to 32nd order (see [Duffy 1981], [Duffy 1980]). 
Methods for analytically handling the positioning of redundant manipulators are 
not even available. 
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A method for recursively computing the end-effector velocity from the 
joint rates was described previously. For a given position & of the manipula- 
tor, the velocity (Vjj + i, oj^+i) of the end-effector link N+l can also be 
written explicity in terms of the joint velocities as 


I ^N +1 ( = [J(6)l £ (*) 

%t-l 1 


Here, [ J (0 ) ] is the Jacobian matrix relating the end-effector motion to joint 
motion. Column i of the 6xN matrix [J] is readily expressed in terms of the 
position results as follows (see [Whitney 1972], [Thomas 1982]): 




(Rotating Joint i) 



(sliding Joint i) 


(The motion of some reference point other than the tool reference origin can 
be used by replacing and Rjg+i by Vp and £p in these equations.) 

For a specified end-effector velocity, Eq. (*) above represents a simul- 
taneous set of six scalar equations linear in the N unknown joint velocities 
These equations will result in either a unique solution, an infinite 
number of solutions, or no exact solutions; they can be solved by standard 
techniques of linear algebra. For example, using the pseudo-inverse [j]“l* 
of the Jacobian 



27 



produces a solution that minimizes the quadratic norm of the error between the 
desired and actual end-effector velocities (see [Lowrie 1982]). In addition, 
where multiple solutions exist the quadratic norm of the joint velocities is 
minimized. This method works quite well in general, although it can produce 
undesirable results when the arm is near a kinematic singularity. 


Kinematic singularities are special configurations of the manipulator for 
which the rank of the Jacobian matrix decreases by one or more (e.g., see 
[Sugimoto 1982]). In these configurations, the end-effector loses degrees of 
freedom of instantaneous motion. When the manipulator approaches such configu- 
rations, the solution techniques previously described can result in excessive- 
ly large velocity specifications for some of the joints. To prevent this, the 
individual joint velocities are constrained by the limits specified during sys- 
tem definition 



The joint velocities are scaled to satisfy this criterion. In the case of re- 
dundant manipulators (including instantaneous redundancy while in a singular 
configuration), linear programming is employed to minimize the magnitude of the 
largest joint velocity, i.e., 

minimize (maximum ( 1 0 . I / e ) } 

^ i,max 

This method produces realistic values for the joint velocity specifications and 
prevents the erratic behavior in the iterative positioning algorithm that can 
result without these modifications. 


Once the joint velocities have been obtained from the end-effector motion 
specifications, the joint accelerations can be evaluated. Finite differencing 
may be employed: 



®i (t) " ®i (t " At) 
At 


or the joint accelerations can be determined from the end-effector motion spec- 
ifications using the relations 


1 %+l 1 

> = [ J] 0 + < 

( v \ 
%+l I 
v ) 

j %KL 1 

■ I 

1 j 


/~Ol- __ - 

Here, aX+i and'4+1 are the velocity-related accelerations of the 
end-effector. These can be obtained from the recursive kinematic method dis 
cussed previously by setting = 0 in those earlier equations. As with the 
velocity transformation, the matrix of coefficients is the Jacobian, and t e 
same linear equation solution procedures previously described can be applied to 

solve for 0. 
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Returning to the problem of finding the joint positions 0 for a prescribed 
end-effector position, an iterative method is implemented in ROBSIM. The al- 
gorithm uses the solution of a set of linear equations, where the Jacobian 
again forms the coefficient matrix, and is described in [Whitney 1972]. Based 
on the current position and desired position of the end-effector, a six- 
dimensional position error vector is obtained. This error vector is used to 
solve for a vector of joint corrections AJ9 from 


( A ^J+1 

/ A i 


[J]A_9_ 


where,ARjq + i is the translational position error and A^ represents a rotation 
vector (the magnitude gives the angle of rotation and the direction specifies 
the axis) that rotates the end-effector from the current to the desired orien- 
tation. A method for solving for the vector is described in the next sub- 
section. Applying the special solution methods previously discussed allows li- 
mits to be placed on the size of the change ,AQ, in the joint angles. This 
improves convergence of the positioning method for large initial errors or when 
operating near singularities. 


After solving for,A£, the joint displacements are updated, 


0 

—new 


■^old 


+ A0 


the current end-effector position is recalculated, and the process is repeated 
until the end-effector position converges to the desired value. Because this 
is an iterative method, only a single position solution will result (depending 
on the initial values of 9 ) , although the manipulator geometry may have multi- 
ple configurations corresponding to the same end-effector position. To date, 
testing has shown the method to be quite robust as long as reasonable limits 
(e.g., 0.1 radian) are placed on the changes in joint displacement per iter- 
ation step. 


Task-oriented motion specification . - This section has presented methods 
for determining link motions from joint motion states and for finding joint mo- 
tions that produce end-effector trajectories. To make task programming more 
convenient, a user- interface is implemented that allows interactive specifica- 
tion of a sequence of operations and robot motions. The motions generated from 
these specifications are described in the following paragraphs. 

The overall task specification is divided into motion segments. The fol- 
lowing options are available for defining the motion within each individual 
time segment: 

1) Rate control, 

a) Joint motion, 

b) End-effector motion (world coordinates and local end-effector 
coordinates); 

2) Position control, 

a) Joint motion, 

b) End-effector motion (world coordinates). 
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For rate control motion, the velocities of the individual position coordi- 
nates are specified as polynominal functions of time 

__ n— 1 

4 = at + a .t + . . . + a. t + a~ 

H n n-1 1 0 

where q is the rate of some position component and the a^ are user-specified 
coefficients. For rate t control of joint motion, q corresponds to the indivi- 
dual joint velocities, 0£. Therefore, the joint velocities at any time dur- 
ing the segment are determined by evaluating the rate polynomials for the given 
time, t. Furthermore, the polynomials can be symbolically differentiated and 
the joint accelerations found by evaluating 

3 = na n tn-1 + (n-1)a n _i tn ~ 2 + • • • + 

The joint displacements are updated using Euler integration of the velocity 
results* 

e (t + At) = e i (t) + At * e ± (t) 

When rate control of end-effector motion is specified, the q polynominals 
correspond to the six components of end-effector velocity, Vp and 01 ^+ 1 . 


(A different tool reference point P can be used for each motion segment.) At 
time t, the rate polynominals are evaluated to determine the end-effector ve- 
locity. These velocity vectors can be defined in either world coordinates or 
instantaneous end-effector coordinates. If defined in local end-effector coor- 
dinates, the vectors are transformed into world coordinates by premultiplying 
each by [ ?m+ I j • The joint velocities are then solved for using the Jacobian 
inversion method described previously. The joint accelerations are obtained 
from these velocities using the backwards finite-differencing equations, and 
the joint displacements are evaluated, as before, from the Euler integration 
equation. 


Position control segments provide coordinated motion between user- 
specified trajectory endpoints. Each position component is scaled so they all 
reach their final values simultaneously. For example, for joint position con- 
trol, the initial joint displacements 0(t o ) are known from the manipulator's 
current location and the final displacements fi(tf) are specified. The joint 
positions at any intermediate time during the motion segment are then given by 

8(t) » £(t Q ) + A — 

where 

Ai = !<t f ) - £(t 0 ) 


*Although symbolic integration of the rate polynomial could be used for this 
case, the Euler integration method must be employed for the end-effector rate 
control specification. 
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Ifttatf “ S “ U “ S '’ Slue e "“ al to *«“« of <*• total motion completed 
u(t 0 ) - 0 
u(t f ) = 1 

zz?: d r l r?' L ° n ? rofue is — 
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by J ° int velocities and accelerations during the motion are given 

l(t) = u(t) A0_ 




Figure 8.- 

Constant-acceleration constant-velocity constant- 
deceleration profile. 
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Position control of the end-effector motion is the most popular type of 
motion specification. At the beginning of the motion segment, a six- 
dimensional vector representing the change in end-effector position and orien- 
tation is calculated. The first three components , ,A£p, define the transla- 
tion of the end-effector reference point during the motion, and the last three 
components, A£, define an end-effector rotation that produces the desired 
change in orientation. The method for calculating this rotation vector is de- 
scribed in the following discussion. 


Recall that each column of the orientation matrix [Pfl+l] defines the di- 
rection of the corresponding local reference axis. The change in direction of 
these unit vectors are the columns of [Ap] where 

[AP] = “ ^N+l^O^ 

Because the rotation axis must be perpendicular to the changes in these unit 
vectors, its direction can be calculated as the crossproduct of any two 
columns* of [AP]. The magnitude of the rotation is found using 


A<J> = cos 


-1 


/ tr<[p N+/ [ipn 


where tr () indicates the trace of the matrix. This equation has two solu- 
tions; the one corresponding to the direction chosen for the rotation axis can 
readily be evaluated. 

As in the joint motion case, the position values at intermediate times 
during the motion are determined using the scaling factor u(t): 


r 


(t) = r^(t 0 ) + u(t) Ar^ 


<f>(t) = u (t) A<(> 


The orientation of the end effector during the motion segment is given in terms 
of <t> as 

iVi (t)I ■ 'WV 1 tDP(t)1 



1 

0 

0 


0 

-a 3 

a 2 

[DP (t) ] = 

0 

1 

0 

+ sin<f> 

a 3 

0 

“ a l 


0 

0 

1 


— a 2 
— 

a l 

0 


+ (1 - cos<{>) 


1-a? 

a 2 a l 

a 3 a l 

a l a 2 

1-32 

a 3 a 2 

a l a 3 

a 2 a 3 

1-4 


*The two columns of [Ap] of largest magnitude are used so special cases are 
handled automatically. 
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where a^, a 2 and are the components, in end-effector coordinates, of 
the unit vector along the rotation axis cj>. With the end-effector position de- 
termined, the inverse positioning algorithm described earlier is used to find 
the joint displacements 9. Then the joint velocities 9(t) can be obtained from 
the end-effector velocities 



The joint accelerations are computed by the backwards difference method. 

The user interface also provides for the specification of such nonmotion 
steps as GRASP, WAIT, etc. These are described in the ROBSIM User's Guide, Ap- 
pendix A. 


Requirements Analysis 


Once the arm motion is established and the positions, velocities and ac- 
celerations of the individual links are determined, the reaction forces and 
torques throughout the manipulator structure can be readily evaluated using 
classical rigid-body dynamics. As in the kinematic analysis, ROBSIM employs a 
recursive method for dynamic requirements analysis because of the method's ef- 
ficiency and tractability (see [Luh 1980], [Hollerbach 1981]). For this analy- 
sis, the forces and torques are successively computed starting from the termi- 
nal link of the arm and proceeding back to the manipulator base. 

Joint reaction forces and torques . - The reaction force and torque 
J:£ at joint i are combinations of the reactions on link i at joint i+1, in- 
ertial loads caused by the acceleration of link i, and any other loads applied 
to the link. Assuming that the only external load applied to the intermediate 
links are the gravity forces acting through their centroids, the recursive 
equations for the joint reactions can be written 


4 " 4+i + m i C ^cg 4 + & 


4 " 4+i + ( 4+i - V x 4+i + hi * m i 4g. + £> 

+ [I - ] a + to . x [I ] a), 
l —I —a. i —x 

The new symbols in these equations are: _ . _ 

m^ - mass of link i; 

a - acceleration of centroid of link i; 

~c gi 

g - acceleration due to gravity; 

[1^] - inertia matrix of second moments for link i about its centroid. 
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All of the vector quantities in these equations are expressed in the world 
coordinate system. The inertia matrix [ £l £] in local coordinates is de- 
fined during system definition, and must be converted to world coordinates 
using the transformation* 

IV " t P i' 'i 1 ! 1 tP / 

The number of multiplications involved in this transformation is a primary 
justification for employing local coordinates in some dynamics formulations 
(see [Luh 1980]). 

Note that the reaction force f^ in these equations is the force link i-1 
must exert on link i for dynamic equilibrium. For the end-effector link N+l, 
the same equations are used except that the applied force ff p and torque _t p 
at the tool reference point replace the terms _f^ + i and (also, Rjj + 2 = 



Actuator drive torques . - Each joint of the manipulator provides a single 
degree of freedom of relative motion between the successive links it connects. 
This relative motion is actively driven by an actuator and drive mechanism. 
Therefore one component of the reaction force or torque at the joint must be 
supplied by the actuator system, while the remaining components are the bearing 
reactions. In addition to providing this joint reaction component, the actua- 
tor must also overcome joint friction and its own inertia. 

The torque (or force+) T \ that actuator i must supply to drive the system is 
given by 

t, = S • t . + e. + K 0. + K o(0 ) (Rotating Joint i) 
l — i —x em, I ev. i ec. i 


f. + I 0.+K 0. + K (7 (6 ) 

— i em. i ev. i ec. i 

x i i 


(Sliding Joint i) 


where the new terms in these equations are 

I e mi “ effective inertia of the actuator and drive system (e.g., 
gearing) ; 

K ev £ - effective viscous friction coefficient; 


K ec i ~ effective coulomb friction coefficient. 

( 1 0 > 0 

The signum function is defined by a(0) =< 

(-1 0 < 0 

*This relation is readily derived from the fact that the rotational kinetic 
energy, a scalar, is invariant with respect to coordinate reference, so &0 


.10? [ I ] .( 0 . = !0? [I. ] u. 
x-x i i x— x — i x — x 


+If joint i is a rotating joint, has units of torque, while for a sliding 
joint i, is a force. The term "torque" will be used in either case. 
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For 0 = 0, o (0) can assume any value between -1 and 1; it is generally selected 
to be zero in this case. 


All of the terms previously defined are effective values referenced to 
joint displacement coordinates to promote computational efficiency. Prepro- 
cessing may be necessary to convert the actual values to their effective va- 
lues. As an example, suppose the actual inertia of the actuator rotor is l am 
and that this motor drives the joint through a gear reduction ratio of n fl 
(i.e., 0 a = n a 0). The effective inertia I em of the actuator motor is 
then given by 

I = n 2 I 
em a am 

The corresponding conversion for the other terms in the actuator torque equa- 
tions are 


K 

ev 


n^ 


K 

av 


K 

ec 


n 

a 


K 


ac 


x 


n 

a 


T 


a 


Presentation of results . - During execution of the requirements analysis, 
print and plot files containing the significant analysis results are generated. 
The print file is a formatted output file that can be viewed at a terminal or 
spooled to a high-speed line printer to provide a hardcopy listing of the re- 
sults, The results saved in the plot file can be displayed on an interactive 
graphics terminal or plotted on a hardcopy plotter. This utility is described 
in the Postprocessing Tools section of this report. 

In addition, the motion of the manipulator system can be displayed on an 
Evans and Sutherland graphics workstation during execution of the requirements 
analysis. A display file can also be saved for subsequent replay. 


Response Simulation 


Response simulation involves evaluation of the motion and force trajec- 
tories of the manipulator system when driven by a prescribed set of actuator 
torques or a specified control law and reference command. In the ROBSIM imple- 
mentation, the dynamic equations of motion are solved for the joint accelera- 
tions at each processing timestep. A Runge-Kutta fourth-order integration al- 
gorithm is used to numerically integrate these accelerations to obtain the 
joint velocities and positions. To calculate the joint accelerations for a 
specified state (position and velocity) and driving torques, the equations of 
motion must be reformulated to explicitly represent these accelerations. The 
appropriate form for the controlling equations is 

1 = [A(0)]£ + b(6,j>) 
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where _r is the vector of actuator driving torques, [ A(0) ] is the effective in- 
ertia matrix referenced to joint coordinates, and b(f),(j) is a vector of 
position- and velocity-related effective torques. The calculation of these 
terms is described in the following subsections. 

Actuator driving torques . - The actuator torques driving the response sim- 
ulation can be generated by one of the following methods: 

1) Read a file of actuator torques versus time; 

2) Read a file of actuator voltages versus time; 

3) Use a feedback control law. 

The first of these methods can be used to determine response to specific torque 
command profiles, such as a sinewave input. Alternately, an actuator torque 
file generated during a requirements analysis run could subsequently be used to 
drive the response simulation, thereby providing validation that the require- 
ments analysis and response simulation agree. 

Figure 9 illustrates some results from a simulation run of this type. A 
set of joint driving torques was generated by the requirements analysis for a 
motion consisting of (1) a straight-line end-effector move for the first se- 
cond, (2) a 0.5-second wait, (3) a 1-second return to the original position 
(joint- interpolated move), and (4) a hold in this position. Figure 9 shows the 
desired trajectory 9(req) for two intermediate joints of a six-degree-of- 
freedom arm, as well as the simulation results for these joints using two dif- 
ferent integration timesteps, 0.01 second and 0.005 second. This figure shows 
how the simulation error tends to increase with time, and that very small time- 
steps are needed to accurately track desired trajectories for long motion seg- 



Figure 9.- . 

Results of simulation driven by torques generated in [requirements analysis 
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Actuation methods 2) and 3) are used with the dc torque-motor model imple- 
mented in ROBSIM. The torque supplied by motor i is proportional to the arma- 
ture current i^: 


where K e ti i- s the effective torque constant of the motor, 
rent is a function of the applied voltage 


V. 

x 


di . 

= L idT + 



i. + 
x 


K 


emf 


0 . 

x 


The armature cur- 


where is the armature inductance, R a £ is the armature resistance and 
K eiJ ifi is the effective back-emf coefficient for the motor. In the motors mo- 
deled to date, the armature inductance has been found to be so small (hence, 
the electrical time constant is very short) that an unrealistically small in- 
tegration timestep is required to track the electrical dynamics of the motor. 
Therefore, the inductance is neglected in the current motor model and the tor- 
que can be related to the applied voltage by combining the above equations to 
obtain 


t . 

x 


V. 

x 


K p 
emf . 

x 


6 . 
x 




(*) 


In this equation, all coefficients are referenced to the joint displace- 
ments coordinates t>. If, for example, the gear reduction ratio is n a , the 
effective values are related to the actual motor coefficients K at and K aem £ 
by 


K = n K 
et a at 


K , = n K 
emr a aemf 


If a voltage file is read, equation (*) is then used to compute the actuator 
driving torque at each timestep. At intermediate times for which no voltage 
has been saved in the voltage file, linear interpolation is used to determine 
the voltage value. 

Rather than reading a previously established file of command voltages, the 
motor drive voltages can be generated during the simulation run using a feed- 
back control law. The implementation of feedback control models in the ROBSIM 
program is discussed in a subsequent sectio n on mani.Bula,tor_jc.ontr oilers. 

i 

Position- and velocity-related torques . - The vector b(£,0) of position- 
and velocity-related torques (see bottom of page 35) included The effects of 
static loads (such as gravity and applied forces), friction, and the velocity- 
related inertia terms ("centripetal" and "Coriolis" torques). Because this 
effective torque contribution depends on the current manipulator state and in- 
cludes all torque terms except the inertia torques attributable to joint accel- 
erations, the vector is conveniently and efficiently computed by performing 
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0 0 

the requirements analysis with the joint accelerations 0 set to zero. There- 
fore no additional program modules need to be implemented for the evaluation of 
this term. 


Effective inertia matrix . - The matrix [A(3')] of position-dependent in- 
ertia coefficients provides the relation between joint accelerations and actua- 
tor torques, including dynamic crosscoupling (i.e., acceleration at joint i 
produces reaction torques at the other joints). This matrix is symmetric and 
positive-definite; the kinetic energy of the manipulator in motion is given by 
the quadratic form 

i k.e. = he? [A(e)3_e 

An efficient algorithm for calculating the inertia matrix, based on the results 
in [Thomas 1982] and [Walker 1982], is implemented in the ROBSIM package. A 
brief presentation of the equations used is given below. 


Consider link i of the manipulator with mass m^, centroid location 
£cgi anc * inertia distribution [I^] (in the current configuration). The ef- 
fective inertia [A 1 ] due to this link's mass distribution is given by 


[A 1 ] = [J 1 ] 1 


[M.] 


[If] 


[J 1 ] 


and where [ MjJ is a 3x3 diagonal matrix with the link mass along the 

diagonal^and where [J ] can be written 

✓ ■ 

n ; ' f i 




x (r 


-eg-. 


S. 

“J 



(Rotating Joint j<i) 



(Sliding Joint j_<i) 


For j „> i, the components of J^ are zero because displacement at joint j 
has no effect on the absolute motion of link i. The total inertia matrix is 
obtained by summing the contributions from each link i. The efficiency of the 
algorithm is further improved by using the symmetry of [A] to reduce the number 
of terms calculated. Also, composite masses, centroids and inertias consisting 


K" ' 



38 



of the mass of all links from joint i to the free end of the arm are computed 
for each i. This allows each term of the expression for [A] to be evaluated 
for only one composite mass instead of for each individual link's mass (see 
[Walker 1982]). 

The mass and inertia of a load being carried by the manipulator are in- 
cluded in the matrix of inertia coefficients by adding them to the mass distri- 
bution of the end-effector. In addition, the effective inertia, I e mi» °f 
actuator i is added to the ith diagonal term of [A] . 

Because the effective inertia matrix is positive-definite, the equations 
of motion 

L = [A (6)36. + b(0,|) 

# • * 

can be solved uniquely to obtain £ in terms of the state (£,£) and driving tor- 
ques £. 

Joint friction . - As discussed in the Requirements Analysis section, two 
types of joint friction — viscous and coulomb — are implemented. The viscous 
friction is readily included in the torque vector £(£,£) as is the coulomb 
friction as long as the joint velocity is not zero. Special methods for hand- 
ling the coulomb friction term must be employed when the velocity 0£ at some 
joint is zero. 

Figure 10 shows the joint coulomb friction t c as a function of joint ve- 
locity. This figure illustrates that when the joint velocity is zero, the cou- 
lomb friction torque value is not uniquely defined, but lies somewhere between 
the minimum and maximum values -K es and K es , where K es is the effective 
static friction coefficient for the joint. In the case when one or more joints 
have zero velocity, the set of equations to be solved for the joint accelera- 
tions can be written 

[a(£) 3 £ = x, - £(£>£) + x 


-k < x < K 
es . — c. — es . 
ii i 


x = p (0 . ,K ,-K ) 

c. i ec. ec. 

i ii) 


where t_ c is the vector of joint friction torques for the zero-velocity 
joints, and the selection function p has the special definition 


p(c,x ,x + ) = 


x ; c<0 

+ 

x ; c>0 

x (arbitrary) ; c=0 
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Figure 10.- Coulomb/static friction at joint. 


A special solution procedure is implemented for solving equations of the type 
given previously; it is briefly described at the end of this section of the 
report. In the case represented here, a unique solution is obtained for Q and 
T C . 

The procedure previously described determines when relative motion at a 
joint begins. Another special procedure must be employed to evaluate when mov- 
ing joints stop because of coulorab/static friction. In the ROBSIM implementa- 
tion, whenever the value of the velocity at some joint switches sign between 
one integration step and the next, the joint velocity is reset to zero and the 
algorithm described above is used to determine whether the joint actually con- 
tinues moving or comes to rest. 

Figure 11 shows the effect of static friction on the motion of a single 
joint driven by a sinusoidal torque input. The response is dramatically affec- 
ted by this nonlinear torque contribution. 

Motion constraints . - The results considered so far assume that the joint 
displacements are independent, controlled variables and that there are no ex- 
ternal constraints on the motion of the end-effector. Often during system 
operation, interactions with the environment constrain the motion of the tool 
(e.g., consider turning a crank or inserting a peg in a hole). Several methods 
exist for modeling these interactions in a computer simulation. For instance, 
if the external forcing functions are known, they can be included in the vector 
as described in the subsection on requirements analysis. 
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Figure 11.- Effect of static friction on system response. 


One method for representing interactions with the environment is by a 
spring between the end-effector and some reference position in the inertial 
coordinate system. The ROBSIM dynamics package contains the capability for in- 
cluding a general compliance element between the end-effector and the fixed re- 
ference. In this implementation, the end-effector position and orientation are 
computed. Then a six-dimensional error vector ('Ar£, AJ>£)T j_ s de- 
termined, which represents the translational and rotational error between the 
actual end-effector position and the spring reference position (in which the 
spring force would be zero). A position-dependent spring force f p and a tor- 
que t^p, given by 


f 


Ar 

~v 

= [K ] 

“P 

t . 
-p 

sp 



are applied to the end-effector. Here, [K sp ] is a 6x6 stiffness matrix. 

These applied loads contribute to the reaction forces and torques at the joints 
and their effects are included in the vector _b in the response simulation form- 
ulation. 
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While this spring formulation is readily understood and implemented, there 
are several drawbacks to using it for modeling motion constraints: 


1) The user must specify a stiffness matrix that accurately char- 
acterizes the system interactions; 

2) It is inconsistent with the rest of the manipulator model, which 
does not include compliance; 

3) The stiffness matrix often contains very large values, thus re- 
quiring a restrictively small integration timestep to assure nu- 
merical stability. 

Therefore ( the ROBSIM simulation tool contains modules for incorporating 
rigid constraints on the motion of the end-effector. The basic form of the me- 
thod implemented has previously been applied for the dynamic analysis of mecha- 
nisms (e.g., see [Chace 1971]), and solves for the constraint reaction forces 
as well as the joint accelerations. To include the effect of contact friction, 
only point constraints are considered. That is, constraints on the angular mo- 
tion of the link cannot be specified directly, although such constraints can 
generally be represented by more than one point constraint. 


Consider, for example, that the tip of the manipulator is pressing against 
the face of a rigid object as illustrated in Figure 12. The tip point r is 
constrained from further motion toward the obstacle. This constraint can be 
expressed as f 1 


. -v ' -r - ° J , where v r is the velocity of the tip point r in contact 

with the obstacle and n r is the outward pointing surface normal. 



This is a single-acting constraint in that motion of the point in only one di- 
rection is restricted. For a double-acting constraint, the component of motion 
along n r is restricted in both directions and the constraint relation becomes 
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This could result, for example, from a roller in a slide or a peg in a hole. 

If the obstacle is moving at a constant velocity, the constraint is handled 
identically, with the component of this velocity along forming the right- 
hand side of these equations. 

Assume that the velocity constraint is instantaneously satisfied exactly. 
To continue satisfying the constraint, the acceleration of point r is also re- 
stricted by the condition* 



From the kinematic relations previously discussed, this can be transformed into 
the following constraint on the instantaneous joint accelerations: 

w T 0 + a V . n = 0 
-r - -r -r 

where 


w = [J ] T n 
-r 1 r -r 

and [J r ] is the translational Jacobian for point r, and aj is the 
velocity-related acceleration of point r (i.e., for the current joint velocity 
state with zero joint accelerations). The constraint on the motion of r is 
provided by a reaction force fj. applied through the point (see Fig. 12). 

This reaction force acts along a line parallel to the constraint direction 


but the magnitude f r must be determined along with the joint accelerations. 
This applied force produces effective joint torques J; r given by (e.g., see 
[Thomas 1982]) 


T 

;-r 


[J ] 
r 


T 

f = f w 
-r r-r 


that must be included in the dynamic equations of motion. 


Consider, for example, the case in which there is one double-acting con- 
straint. The above relations lead to a set t> of N+l linear equations that can be 
solved for the instantaneous accelerations 8 and reaction force magnitude f r 


[A] ! 

w 

— — r 

(J) = 

, t - b 

T 1 

0 

j-f j 

v 

-a • n 

— r 1 

L i 


{ r) 

-r -r 


*For the single-acting constraint type, replace the equality sign by "greater 
than or equal to" in these relations. 
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Configurations containing single-acting constraints can be handled by em- 
ploying the nonlinear, partial constraint function p defined previously in the 
subsection on joint friction. In this case, the equations to be solved for the 
joint accelerations and reaction force are: 

[A] 0 = T - b + f r w r 

f > 0 

r - 

w^9 + a V *n > 0 

-r - -r -r - 


. f = p(w T 9 + a V . n , -, 0) 
r ~r — -r — r 

The implicit function p is introduced to represent the fact that the reaction 
force must vanish as the point accelerates away from the constraint. (Note 
that the conditional cannot be less than zero in this case.) 


The solution procedure described at the end of this section is again em- 
ployed to solve these equations. In general, a unique solution will exist. 
However, if some joints have zero velocity and some constraints are active, the 
reaction force distribution may be indeterminate, although a unique set of 
joint accelerations will result. More than one motion constraint can be con- 
sidered simultaneously by simply expanding the set of equations. 

Impact of collision . - In the above equations, it was assumed that the 
point velocity already satisfied the motion constraint. However, when the 
point initially collides with the obstacle, this will not be true. In the act- 
ual case, this collision will introduce oscillatory transients in the motion. 
This rapid transfer of momentum can be modeled as an inelastic collision to de- 
termine the velocity of the system after impact. To evaluate the joint veloci- 
ties after impact, consider the generalized momenta which are given by 


q = [Ale 


where [A] is the inertia matrix defined earlier. The change a £ in generalized 
momenta must equal the generalized impulse Tj that results from the impul- 
sive reactive force at the constraint. If the reaction impulse has magnitude 
fj, then the generalized impulse will be 


ii 


IJ r )T £ I Sr ' £ I -r 


This leads to a set of linear equations for determining the change A0 in the 
joint velocities during the impact: 


AQ = 





A0 = 


f 


I 


w 

-r 


44 



Combining these equations with the fact that the velocity constraint must be 
satisfied after impact leads to the following set of equations for obtaining 
the joint velocities after the collision 



" [A] ! 

w 


A0 


0 


1 

_-_r 




j 

“ T T 

L» r 1 
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If V 

1“ 

~-r* -r 


• -after -before + 


where v r is the velocity of the constraint point before impact. 

Coulomb friction at the constraint points . - In addition to the normal re- 
action force at the point of contact with the obstacle, there is also a fric- 
tion force tangent to the restraining surface. This friction force, ff, is 
proportional to the normal force and directed opposite to the tangential velo- 
city, i.e.. 



-y f |e 
M r -r 


where ji is the coefficient of friction and ^ is a unit vector in the di- 
rection of tangential motion of the constrained point relative to the surface. 
The effective torque contribution at the joints is again found by premulti- 
plying the friction force by the transpose of the Jacobian. For the single- 
acting constraint type, the equations are the same as given earlier, except the 
term f r w r is replaced by f r (w r -jiWf ) where 



e 

-r 


In the case of a double-acting constraint, slightly more modification is 
needed because the magnitude of can equal plus or minus f r . This case 
is put into the standard form used above by replacing f r with two variables 
f£ and f~, one of which is always zero depending on the sign of f r . 

The joint torque contribution from the reaction normal force and friction force 
can be written 


T 

-r 



(w r -yw f ) + f (w r +yw f ) 


The additional constraints for this case are 



0 


-f >0 
r - 

t~ = P (f r + , -, o) ; 

where the selection function P implies that f r = 0 if f£ > 0 and 
f” has an arbitrary value (i.e., must be solved fom other constraints) 
when f£ = 0. 
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As discussed for the joint friction case, the real difficulty in handling 
coulomb friction occurs at the singularity when the relative velocity between 
the interacting surfaces is zero. However, the ability to simulate this case 
is critical. For example, consider the insertion of a peg in a hole. It is 
vital to determine whether the insertion will proceed in the presence of cou- 
lomb friction, or whether the peg will instead jam in the hole. If jamming 
does occur, will the manipulator be able to free the peg and continue inser- 
tion? 


To model the case of coulomb friction with zero tangential velocity, the 
friction force is approximated by two mutually perpendicular forces 


u 




) 


where (:£ and e£ are two mutually perpendicular unit vectors that are 
also perpendicular to the surface nprmal (i.e., they define the instantaneous 
tangent plane). The coefficients c£ are bounded by the magnitude of the 
normal force 



Additionally, if the contact point has nonzero tangential acceleration, the 
friction force will have its limiting magnitude so 


and 


^ 1 = o 1 If I - If I) ; 1=1,2 

~ r » 1 r i r i ' ’ » 


a^.e^ = (w/) 1 0 + £ r .e r ; i=l,2 


The magnitude of the normal force for the single- and double-acting constraint 
types for use in these relations is given by 

: | f r I = f r ( sin g le-actin s) 


|f | = f r + - f r (double-acting) 

This method introduces two additional unknowns (c£) and six additional 
constraints (four linear inequalities, two nonlinear partial constraints), all 
in the standard form developed previously. Some results using these equations 
are presented in a subsequent section of the report. 
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Modeling the ISRL Multisensor End-Effector 


One component of today's manipulators that is woefully inadequate compared 
to the dexterity and sensitivity of its human counterpart is the manipulator 
end-effector or hand. Advanced mission applications will surely require more 
sensing capabilities than employed in industrial robots, which typically have 
at most a single presence detector, and more handling dexterity than the uncon- 
trolled opening and closing of simple pneumatic grippers. This section de- 
scribes the simulation of advanced end-effectors that incorporate several types 
of internal and external sensing and controlled gripping actions and discusses 
a specific implementation — the microprocessor-controlled gripper/ sensor system 
developed for and in use at the Intelligent Systems Research Laboratory (ISRL) 
at NASA-LRC. The end-effector consists of a parallel jaw gripper mechanism 
with force sensors and proximity sensors mounted in the jaws and is attached to 
the manipulator through a six-degree-of- freedom wrist force sensor. The simu- 
lation model includes the geometry and dynamics of gripper operation, influe- 
nces of load inertias on arm operation, wrist and jaw force sensors, and jaw- 
mounted proximity sensors. 

Configuration of the ISRL end-effector . - Figure 13 displays the ISRL end- 
effector and Figure 14 illustrates the functional components of the gripper. 

The end-effector mechanical design and concept for proximity detection origi- 
nated at the University of Rhode Island. The body of the end-effector houses a 
dc torque motor. A nylon gear on its shaft drives an incremental shaft encoder 
and a dc tachometer providing position and rate feedback; the gearing provides 
a 1:2.78 speed increase. The encoder output is wired directly into the micro- 
processor controller and the tachometer output is summed with the position 
error signal in the servoamplif ier (Fig. 14). A limit sensor indicating the 
fully open configuration of the jaws provides a reference for the incremental 
position encoders. A worm gear on the motor shaft drives two opposing sector 
gears, each of which is attached to a link in a parallelogram mechanism that 
provides translation of the corresponding jaw, keeping the jaws parallel during 
opening and closing. The worm gear ratio is 100:1. 

In addition to the gripping mechanism, motor and sensors, the ISRL end- 
effector incorporates proximity sensors in each jaw, a crossfire presence de- 
tector, multifreedom strain gage force sensors at each jaw mount, and a six- 
degree-of-freedom wrist force sensor. These sensors are described in subse- 
quent sections. Also, the end-effector mechanism contains a spring-loaded 
mounting base with a limit sensor that provides overload protection when the 
device is in operation. 
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Geometry of gripper operation . - The ISRL end-effector contains one degree 
of freedom, specified by the reference angle g r shown in Figure 15. This is 
the angle between the reference z-axis and the centerline of the supporting 
gripper links. The motor angle, encoder angle and tachometer rate are related 
to the gripper angle by 


1) Motor angle = 100 *g r ; 

2) Encoder angle = 278 (^ r ; 

3) Tachometer rate = 278 

The end-effector reference system is chosen with the z-axis directed outward 
from the gripper along the motor axis, and the y-axis in the plane of motion of 
the jaws 9 (Fig. 15). The origin of this coordinate system is chosen as the 
midpoint of the wrist force sensor so the reaction force computed for this 
point can be used for modeling the wrist sensor results. Local reference sys- 
tems are associated with each jaw of the gripper for defining the motion of 
such jaw-mounted components as the proximity sensors. Because the jaws do not 
rotate relative to the gripper body, their coordinate system remain parallel to 
the end-effector reference. The vectors N+lilLJ and N+liiRJ define the 
instantaneous location of the jaw coordinate systems relative to the end- 
effector reference and are given by the relations 


N+l-LJ = — LJ + 1 


0 

- sin0 gr 

COS0g r - 1 


N+l-RJ 



l 


0 

sin0g r 
cosGgr - 1 


where hf^ is the vector in end-effector coordinates to the tip of the 
left jaw when the gripper angle 0 gr is equal to zero, etc and C is the length 
of the connecting link (Fig. 15). The distance d gr between the gripper jaws 
can be expressed in terms of the gripper angle as 
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sin0 

gr 


where dgjf^ is this distance in the reference position ( 0g r = 0). 
positions of the gripper jaws in terms of world coordinates are 


The 

given by 


h+1 + [P N+l ] N+l\j 
= ^N+l + [P N+1 ] N+l-RJ 
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Then, any point whose position ljt in jaw coordinates is known (e.g., a prox- 
imity sensor fixed in the jaw) and can be located in world coordinates by the 
standard point transformation. 

£ * + tP N+l ! <uS> 
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Gripper dynamics . - The dynamics associated with the single-degree-of- 
freedom gripper operation can be characterized by a second-order piecewise lin- 
ear system. Figure 16(a) shows a block diagram representation of this system; 
it includes analog rate feedback from the tachometer, viscous friction, a cou- 
lomb friction term and the jaw gripping force. The symbols in this diagram in- 
clude: 

^dac “ output voltage from the digital-to-analog converter 

K amp ~ servoamplif ier gain 

^emf - back emf coefficient for the gripper motor 

^tach ” tachometer coefficient relating tach voltage to gripper speed 

K ev • ~ effective viscous friction coefficient 

K gc - effective coulomb friction coefficient 

Kj - effective torque constant for the motor 

Rjj, - motor circuit resistance 

Ig r - effective inertia of the gripper motor, gears, linkage and jaws 

Fg r - gripping force 

Tp - effective torque due to gripping force 

s - Laplace variable 

The gripper angle ;e gr forms the reference coordinate for most of these terms 
(Fig. 16); for example K ev gives the torque referenced to 0g r corresponding 
to velocity !0g r » The voltage from the digital-to-analog converter is deter- 
mined by the control law implemented in the microprocessor controller, gripper 
angle feedback from the digital encoder and a reference angle supplied by a 
user or host computer. 
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i(a) Explicit form. 



'^(b) Simplified form. 

/Figure 16.- Block diagram representing the dynamics of gripper operation. 


The terms in Figure 16(a) can be combined to form the simplified block 
diagram shown in Figure 16(b). The new coefficients in this diagram are de 
fined in 



(K-Iach ^ am P + ^emf ) + ^ev 








The digital-to-analog converter acts as a zero-order hold, so v ( j ac is a 
piecewise constant voltage. Assuming that the grippers is not contacting a 

workpiece and that the gripper velocity does not change sign ( so that T c is 

a constant), the dynamic equation represented by Figure 16(b) can readily be 

solved to obtain the time trajectory of ’0g r during each period of constant 

voltage. The controlling equation is 


and the solution can be written 


0(t) = 


Kdac Vdac ~ T c ^ _ e - ?(t-t 0 )^ + • g - £(t-t Q ) 


l 0(0 = 0 O + 


Kco 

Kdac ^dac “ T 




c [(t-t 0 ) + ~ (e " C(t " to) - 1)] 


- 0 o T (e 


l , - ;(t-t D ) 


O' _ 


1 ) 


where tg is the time at the beginning of the segment, 10 g and |0g are the 
position and velocity at the tg and 


5 = 


V 


In a computer simulation, these equations can be used to evaluate the transi- 
tion in the gripper state during one timestep of its controller unless the 
gripper velocity changes sign or the gripper closes on a workpiece during this 
transition. In either of these cases, the driving torque (right-hand side of 
the differential) changes during the timestep. 

If the gripper jaw motion reverses direction, the time at which this oc- 
curs can be obtained from the previous velocity equation. The gripper position 
at that instant is determined and then the transition continues with the new 
values for the coulomb friction term, tg, 0g and 0g. When the gripper 
closes on an object, the same procedure can be employed, but the differential 
equation is changed to include a torque term proportional to the distance be- 
tween the gripper jaws to simulate the compliance in the jaws and object. A 
simpler approach that can be used to evaluate the steady-state gripper force 
corresponding to a given applied voltage involves modeling the gripper impact 
as an inelastic collision between rigid bodies. The effective torque T p due 
to the gripping force Fg r is given by 

X., .= K, V, 

F dac dac 

The gripping force is found from 


X F “ F gr G gr 
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where Gg r is the first geometric derivative relating the closing of the jaws 
to the gripper reference angle and can be obtained by differentiating the posi- 
tion relation, leading to 

Gg r — 2£cos0g r 

Grasping a load object . - Grasping an object can lead to constraints on 
the arm motion (if the object is constrained) or additional mass to transport 
during the motion. Suppose a load is rigidly grasped by the end-effector and 
is moved by the arm. The path of the load object is found by evaluating a re- 
lative translation ^ + i h$ and rotation [j^+lPfil between the end-effector 
reference and the load object's coordinate system at the time of the grasp 

iN+l-Jl = [P N+1 J - 4j+ 1 ) 

[ N+1 P £ ] = 

Then, after evaluating the end-effector position for each subsequent arm con- 
figuration, the load position can be determined using these relative positions 
by rewriting the equations as 

= —N+l + [P N+l^N+l-£ 


lP Jl ] [P N+1 ] f N+l P £.^ 


The dynamic effects of the load mass on the arm motions are modeled by 
adding the mass distribution of the load to that of the end-effector. The po- 
sition of the load relative to the end-effector is used in the procedure for 
combining the rigid-body masses described earlier in the section that discusses 
the system definition function. 


The simplest implementation of the grasp operation performs these steps 
regardless of whether the object was in position to be gripped and the grasp 
was successful. To determine the success or failure of the grasp, knowledge of 
the surface geometry and positions of the object and gripper jaws must be used. 
For example, consider a cylindrical peg being grasped by flat fingers (Fig. 

17). The position of the peg is determined by the vector £ p to the center of 
one end of the peg and the direction cosine vector Sip along the peg axis 
(Fig. 17). The projection of the peg axis onto the plane containing the jaw 
surface is readily evaluated by transforming the coordinates of this axis into 
the local jaw reference system (Fig. 18). The critical points c^ and C£ 
that define the endpoints of this projection onto the jaw surface can then be 
found. If either end of the projection of the peg axis lies within the jaw 
surface, this is one critical point. Other critical points occur at the inter- 
section of the peg axis projection with the edges of the jaw surface (Fig. 18). 
These critical points must be determined to evaluate the load distribution be- 
tween the two gripper jaws. If there is no intersection between the peg axis 
projection and the jaw surface, the grasp fails and corrective action must be 
taken. 
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Figure 17.!- Location of cylindrical peg 



Figure 18.- . 

/Critical contact points between peg 
■and jaw surface. 


End-effector force sensing . - The ISRL end-effector is mounted to the arm 
through a six-degree-of- freedom force/ torque wrist. By placing the reference 
point for the end-effector coordinate system at the center of this sensor, the 
force and torque computed during analysis for this reference point are the 
force and torque measured by the six-axis sensor. 

In addition to the wrist sensor, the ISRL end-effector incorporates force 
sensors in each jaw of the gripper. These force sensors measure forces and tor- 
ques in the local x- and y-coordinate directions of each gripper jaw (see Fig. 
15). The distribution of applied loads between these two jaws depends on the 
critical contact points previously identified. The applied load on the peg is 
balanced by reaction force and _fc2 at the critical contact points Cj 
and C 2 « The components of these forces in the local z-direction are neglec- 
ted because the jaw force sensors have no sensing capabilities along or about 
this axis. The components in the local x-direction are assumed to be evenly 
distributed between the left and right jaws, and the y-axis force at each point 
acts on only one jaw depending on its direction. The y-coraponent of the reac- 
tion forces at the jaw contact points also include terms for the gripping force 
applied. For example, consider a force of magnitude f a applied to the peg as 
shown in Figure 19(a). This leads to reaction forces f^, supplied by the 
right jaw, and Fg2> supplied by the left jaw. In addition, a gripping force 
of magnitude f„ r is applied. Figure 19(b) shows how these reaction forces 
are distributed between the left jaw (f^i and f^) and right jaw (f r i 
and f r 2 ) as the applied force magnitude varies. The reason that reaction 
forces greater than the gripping force can be supported is that the gripper 
worm gear is not backdriveable. 

The jaw reaction forces generate forces and moments at the jaw force sen- 
sors. These forces and moments can be used during arm operation to identify 
the critical contact points and the external loads applied to the peg, although 
forces along, and moments about, the local z-axis cannot be identified, nor can 
the local x-coordinates of the critical contact points. 
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POSTPROCESSING CAPABILITY 


Introduction 


This section describes the capabilities implemented in ROBSIM for process- 
ing data generated during execution of the analysis tools functions. Because 
enormous amounts of data may be generated, methods other than viewing printouts 
are necessary to evaluate these data and compare different analysis runs. The 
following options are included in ROBSIM to accomplish this: 

1) Graphics replay of simulation motion; 

2) Graphics replay of simulation vs hardware motion; 

3) Plots of manipulator parameters vs time. 

Items 1) and 2) use an Evans and Sutherland picture system to display arm 
motions. The replay of simulation motion displays the manipulator system mo- 
tions as they occurred during a requirements analysis or response simulation 
run. Replay of simulation vs hardware allows the user to display the motion 
of actual hardware (a "real" arm) and the motion of its software model simul- 
taneously. The simulation model is displayed in the configuration defined 
during -system creation. The hardware arm is shown as a stick figure super- 
imposed on the simulation model. It should be noted that no calculations or 
analyses are carried out during either simulation replay or simulation vs hard- 
ware replay other than those needed for the Evans and Sutherland display. 

Manipulator parameter plots allow the user to create x-y plots of various 
manipulator parameters as functions of time. Some of the parameters available 
for plotting are joint and end-effector positions, velocities, accelerations, 
forces and torques. These plots may be displayed on a graphics terminal or 
drawn on a pen plotter. 


Simulation Replay 


Replay of a manipulator system simulation displays the motions of the sys- 
tem exactly as they occur during a requirements analysis or response simulation 
run. The system is depicted as defined during system creation/modification 
with simple cylinder components or detailed geometries. Simulation playback 
makes use of a data file, written during the analysis function, that contains 
manipulator joint displacements as functions of time. These, and data defining 
the system geometry, are used to locate the positions and orientations of all 
joint/link pairs and load objects held by an arm with respect to the world 
coordinate system. The methods used to find these positions and orientations 
were described in the System Definition and Analysis Capabilities sections. 
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Because very few calculations are needed for the playback compared to the ac- 
tual analysis, the motion is more continuous. As the playback proceeds, the 
motion may be halted at any time by pressing the button labeled zero on the 
Evans and Sutherland control panel. The display of the manipulator system may 
then be moved or rotated using the Evans and Sutherland control dials. This 
allows the user to view the system from a different perspective. Pressing the 
zero button again resumes the playback motion. 


Simulation vs Hardware Replay 


This postprocessing option allows the user to display the motion of a sim- 
ulation as previously described, and concurrently display the motion of a hard- 
ware arm superimposed on the simulation. This is quite useful when validating 
system models. The data collected from a run of a hardware arm consist of 
joint actuator voltages and joint positions as functions of time, stored in a 
file, and transferred to the mainframe computer. A response simulation run 
modeling the hardware arm may use the hardware joint actuator voltages as con- 
trol inputs or the same control algorithm as used for the hardware. This makes 
joint displacements as functions of time for both the hardware arm and its sim- 
ulation model available for this option of the postprocessor. The joint dis- 
placements are used to find positions and orientations of the system components 
by the methods described earlier. The Evans and Sutherland picture system then 
uses the position and orientation data to display the motion of the two arms. 
The model, or simulation arm, is displayed as depicted during system defini- 
tion; the hardware arm is displayed as a series of straight lines representing 
the centerlines of the arm links. As with the simulation replay described ear- 
lier, the motion of the display may be stopped at any time by pressing the zero 
button on the Evans and Sutherland control panel. The motion is suspended and 
the display may be viewed from a different perspective. Motion is resumed by 
pressing the zero button again. Figure 20 shows an Evans and Sutherland dis- 
play of a planar arm simulation model and the stick figure representing the 
hardware arm. 


Parameter Plots 


This postprocessing option allows the user to plot various arm parameters 
as functions of time. A commercially available graphics software package is 
used to display the plot data on either a graphics terminal or a pen plotter. 
The choice of parameters available for plotting depends on the plot package 
chosen during the requirements analysis or response simulation run. The plot 
packages available for each type of analysis are tabulated. 
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Requirements analysis 

Response simulation 

1) Brief 

2) End-effector data 

3) Joint positions 

4) Reaction forces 

5) All or the above 

1) Brief 

2) End-effector data 

3) Joint positions 

4) Reaction forces 

5) All or the above 

6) PID control data 

7) Force/torque control data 



Figure 20. - Simulation vs hardware replay. 











Listings of the manipulator variables saved by each plot package may be found 
in the analysis tools and postprocessor user's guides. Data pertinent to the 
chosen package are written to a file during the analysis run. During postpro- 
cessing, the user may plot any of the parameters saved in this file. The user 
determines the plot title, x- and y-axis labels and the number of curves per 
plot (up to 31). Figure 21 shows the displacements of joints 1 and 2 for a 
planar arm during response simulation using PID control. 



Figure 21. - Joint displacements. 
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MANIPULATOR CONTROL 


As the number of manipulators used in various applications increases, the 
demands placed on control systems and sensor performance also increase. The 
areas in which the demands occur are accuracy (implying repeatability), speed, 
and uniformity of performance over a range of loading conditions. Efficiency 
of operation — implying the use of optimization techniques — is also becoming im- 
portant, especially with regard to long-duration space missions and high-volume 
industrial operations. This section focuses on the different families of al- 
gorithms used for manipulator control. 

In the Manipulator Description section the basic manipulator problem is 
defined in terms of fundamental elements — dyanmics, reference frame transforma- 
tions, and performance requirements. The Control Algorithms section describes 
several conventional position control approaches. The control algorithms dis- 
cussed are: 

1) Linear discrete with feedforward nonlinear compensation; 

2) Resolved acceleration; 

3) Resolved motion force. 

The Adaptive Control section discussion includes both linear adaptive con- 
trol and nonlinear adaptive control. Simulation results are presented for both 

CSS 65 • 

The Force/Torque Control section introduces the subject of control invol- 
ving both positional and force constraints. Two approaches, hybrid control and 
active stiffness control, are analyzed in detail and simulation results pre- 
sented. 


Manipulator Description 


A full description of the plant to be controlled is required as the first 
step in any control analysis. This section describes the properties of the 
manipulator important to control system synthesis: 

1) Kinematics; 

2) Dynamics; 

3) Performance requirements. 
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Figure 22 shows a typical manipulator system. 



Figure 22. - Six-degree-of-f reedom manipulator. 


Kinematics . - The first area of concern, kinematics, involves transforma- 
tions between the manipulator reference frame, the joint angles and linear dis- 
placements (in the case of sliding joints), and other Cartesian reference 
frames (work coordinates). Although the transformation equations are not diff- 
icult to determine conceptually, the function involves a significant amount of 
computation. After this, the problem then becomes one of determining a torque 
input vector T so actual joint positions will match desired joint positions 
with specified time and overshoot constraints (Fig. 23). 



Figure 23. - Manipulator joint position solution procedure. 
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where 


is the desired end-effector position (in Cartesian coordinates) 

X a is the actual position of the manipulator (in Cartesian coordinates) 

$ a is the vector of actual manipulator joint angles 

T is the vector of torque inputs to be applied to the manipulator joints 

This is the typical servomechanism control problem. The direct applica- 
tion of standard linear control theory is not possible, however, because of the 
nonlinearities and coupling inherent in the dynamics of the manipulator. A 
following section describes the basic methods employed for constructing the 
dynamic equations of a manipulator, the basic form of the manipulator equa- 
tions, and the equations for the two-degree-of-freedom model used in the con- 
trol algorithm comparisons discussed later in this report. 

Performance requirements are the final area of concern in establishing the 
framework of the manipulator control problems. Typical requirements and their 
impact on formulation of the control problem are discussed in the final sec- 
tion. 


Manipulator dynamics . - The problem of manipulator control is made con- 
siderably more difficult by the nonlinear, highly coupled nature of the dynamic 
description of a general manipulator system. 


xn Figure 


The dynamic equations for a three-link planar arm are showr 
24(a) and (b) . These equations are included to demonstrate that, even for a 
relatively simple dynamic system, the dynamic equations become complex very 
quickly. 


Performance requirements . - Performance requirements for a manipulator 
can take several forms. In the simplest case the requirement consists of 
tracking some reference trajectory in space with the end-effector. A time- 
space trajectory includes the possibility of position, velocity, and ac- 
celerat ion” constraints . Because these constraints are usually specified in 
terms of a Cartesian work frame, and the control is usually in terms of mea- 
sured joint variables, coordinate transformations are required to relate the 
two. 


A second general class of requirements can be stated in terms of forces 
or torques applied at the end-effector. As in the case of position and atti- 
tude relating to joint positions, end-effector forces and torques are related 
by coordinate transformations to joint torques or forces [Wu 1982]. 

To perform a general task (peg- in-the-hole insertion for example), both 
position and force constraints must be met simultaneously. This subject is 
discussed in the final section in greater detail. 
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Definition of Variables 


Motor Parameters 


n . 

1 


K. 


Ti 


K. 


V. 

1 


K 


Bi 


= Motor Shaft Angle 
Joint Angle 


= Torque Constant Of The Motor (Nm/amp) 


= Amplifier Gain (V/^) 

=? Amplifier Input Voltage (V) 
= Back emf Constant (V/rad/s) 


= Armature Resistance (ohms) 


ai 

LINK PARAMETERS 


m. 

1 


1. 

l 



= Mass Of Link (kg) 


= Distance from Joint To Center Of Gravity (eg) Of Link (m) 
= Moment Of Inertia Of Link about eg (kg-m 2 ) 

- Length Of Link (m) 


ei 


= Effective Moment Of Inertia Of The Gears and The Rotating 
Portions Of The Motor, Measured At The Joint (kg-m^) 


Figure 24(a). - Three-link planar arm. 
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Although simplistic, this system retains the basic difficulties of a higher 

degree-of- freedom system as evidenced by the nonlinear terms. The only term 

missing from this equation that is found in more general systems is a force 

term attributable to Coriolis acceleration. The Coriolis term is manifested as / 

the product of angular velocities, (0.,e ) and is therefore very similar to the ^ 

centrifugal terms; hence, no generality Is lost in a controller designed for 

the planar system. The basic form of the controllers is shown in Figure 2b. 


COMPENSATION 

TERMS 



Figure 26. - General controller structure. 

The compensation terms are discrete approximations to the nonlinear terms in 
the manipulator dynamics. Typical approximations used for compensation are 
shown in Table III. 


Table III - NONLINEAR APPROXIMATIONS 


1 ACTUAL 

DISCRETE APPROXIMATION 

t a 

m^a^LcosCQ^-ep 02 

m 2 a 2 Lcos(e 2 (k) -6 l (k) ) (0 2 (k) -2© 2 (k-l)+0 2 (k-2) ) 

t2 

t g 

(m^aj+m 2 L)gsin0 ^ 

T 

gsinO ^ (k) 

T c 

. 2 

m 2 a 2 Lsin(0 j-0 2 ) 0 2 

m 2 a 2 Lsin(6 1 (k)-e 2 (k)) (0 2 (k)-0 2 (k-l)) 2 

T 

/ 
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These nonlinear compensation terms are meant to provide "correcting" tor- 
ques. In the ideal case these corrections are exact and the dynamic equations 
can be rewritten as: 


(j)e + (K)e + 

r 

!g< e i» e 2> 

+ 4 (9 2 

> 6 1*V = ^e + ^g + 4- 

where 




J 12 0 2 = T A1 

J 21®1 

= t a 2 ' 


^gl = t gi 

f 9 
g2 

= T G2 

\ 

■\ 


Canceling these equalities leads to two decoupled linear systems. 


J 11 0 1 + Vl = t ei 


J 22 0 2 + K 2 6 2 T E2 


Any conventional linear, discrete design technique may then be used to design 
the linear compensator shown in Figure 30. Basically, these techniques are: 

1) Pole-zero compensator in conjunction with root locus using pole place- 
ment: 

2) Pole-zero compensator corresponding to lead, lag, and lead-lag designs 
in the frequency domain using gain margin, phase margin, resonance 
peak, and bandwidth specifications: 

3) PID design leading to closed loop pole placement. 

Items 1) and 2) are shown in Figure 27; Item 3) is shown in Figure 28. 


^ 

r\ — ^ E(z > 

D(Z) = K(Z-Z^(Z-Z 2 )... 

u(z >-2\ 


r . i 

(Z-P^(Z-P 2 )... 



V z) 

Figure 27 . - Pole-zero compensator. 
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INTEGRAL 



Figure 28. - FID controller. 

Design techniques for selected compensators are described in more detail 
in the following paragraphs. Two key questions must be examined: 

1) What impact does neglecting compensation terms have on control system 
performance? 

2) What impact do additional loads with nonadjustable compensation have 
on performance? 

As shown previously, the use of nonlinear compensation terms simplifies 
the problem to a standard, linear, digital design. Therefore, the first step 
in the design process is to develop a discrete model for the decoupled plant. 
Taking the Laplace transform of each term 

(J uS 2 + K,s)e 01 (s) - t e1 (s> 

(J 22 S + K 2 S)e 02 (S) = t e2 CS) 2 
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leads to the required z-transform / 

G(Z) - Z^J. z ( 1 ) 

Z S"(JS + K) 

The required Z-transform [Franklin 1981] 

2 \= Z_((aT - 1 + e~ aT )Z + (1 - e ~ aT - aTe~ aT ) ) 

\ s2 (s+a)/ a(Z - 1) Z (2 - e ~aT) 

yields 

G < z) 


J (aT - 1 + e~ aT )Z + (1 - e aT - aTe aT ) a = K/J 

~ K 2 (Z - 1) (Z - e _ai ) 

This equation is further modified so that the impulse response of the discrete 
model matches that of the continuous system. In other words, 

LIM S G(S) = LIM(Z-1)G(Z) , 

S-K) Z-*l 

by the final value theorems for both Laplace and Z transforms. This requires 
an additional factor of 1/T. Therefore, 

mns = JL ( aT - 1 + e~ aT ) Z + (1 - e~ a -aTe~ aT ) 

. aKT ( Z - 1)(Z - e-aT) 

( aT - 1 + e~ aT ) Z + ((1 - e~ a — aTe~ aX ) / (aT - 1 + e~ aT ) ) 
aKT (Z - D(Z - e -aT ) 


This yields a pole-zero diagram in the Z-plane as shown in Figure 29. 






There 
Figure 29: 
1981] . 


are two basic approaches for the design of the compensator shown 
root locus or frequency domain design in the W-plane [Franklin 


in 


In the root locus approach the compensator is used to alter the loop 
transfer function through pole cancellation, followed by a gain adjustment to 
achieve the desired closed-loop pole placement. This is shown in Figure 30. 



Figure 30. - Root locus with pole cancellation. 

The second compensator design technique explored uses the PID controller 

shown in Figure 28. The three terms in this controller can be combined to form 
a general G(Z) 

a(z) = K i z + K p( z ~ l)+(K D /T)(Z - l) 2 

(z - l) 

The three adjustable parameters, K x , K P , and k d allow arbitrary 
placement of the poles of the closed-loop system. y 

The PID controller has another important property with regard to disturb- 
ance rejection (Fig. 31). 
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R(Z) 


W(Z) 



Figure 31. General control loop with disturbance, W(Z). 
The input/ output transfer function is 

\ 

\ ' Y(Z) _ D(Z) G(Z) 

R(Z) 1+D(Z)G(Z) 


The disturbance transfer function is 

J 

Y(Z) = G(Z) 

W(Z) 1+D(Z)G(Z) 

Let, G(Z) = ni(Z) D(Z) = n 2 (Z) then » then 

^l(Z) 

>' Y (Z) = W(Z) d l< z > 

1 + n 1 (Z)n ? (Z) 

(Z - lJdjC'Z) 

! - w(2> < z - 

(Z - Udjtz) + ni (Z)n 2 (Z) 


This shows that if W(Z) is a step disturbance 

W(Z) = z 
Z-l 


then 

Y (Z) = Z(n^ (Z) ) 

(Z - l)d 1 (Z) + ni (Z)n 2 (Z) 


Y(Z) 
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If the denominator has roots inside the unit circle, the steady-state re- 
sponse to a step, disturbance is zero; i.e., there is no steady-state "offset." 
This is attributable to the pure integrator in the controller. In contrast, 
assume (Z-l) is replaced by some general d 2 (z) in the disturbance transfer 
function. Then for a step disturbance 

Y (Z) = Z d 2 (Z)n 1 (Z) 

Z-l d 1 (Z)d 2 (Z) + n 1 (Z)n 2 (Z) 

The steady-state contains a step component 

Y (Z) = LIM Zd 2 (Z)n 1 (Z) 1 0 

step Z->1 d 1 (Z)d 2 (Z) + n 1 (Z)n 2 (Z) 

This implies that there will be a steady-state error to a step disturb- 
ance. The physical interpretation of these results is straightforward. At 
equilibrium, in the non-PID case, a non-zero error is required to provide the 
torque necessary to maintain equilibrium. In the PID case, the error can be 
zero while the output from the integrator provides the torque to counter the 
disturbance. From the standpoint of a manipulator system this is an interest- 
ing property because gravity disturbances are essentially step disturbances 
around a given operating region. 

Resolved acceleration control (RAC) . - The RAC is a method of position 
control of a manipulator. The algorithm uses the position and orientation, 
the velocity, and the acceleration of the hand (or end-effector). The desired 
position and orientation, velocity, and acceleration of the hand are specified 
at each time step, and the control algorithm determines the joint positions, 
velocities, and accelerations required to satisfy the specified conditions. 
Finally, the input forces and torques to be applied to the joints are calcu- 
lated using the "inverse problem" technique [Luh 1980a]. 

One advantage of the RAC method is that the forces and torques are not ob- 
tained by the dynamics equations; rather they are calculated as a function of 
the desired and actual joint positions, velocities, and accelerations, using 
the Newton-Euler formulation of the manipulator equations. The method succes- 
sively transforms the velocities and accelerations from the base to the hand, 
using moving coordinate system relationships. The input forces and torques are 
computed for each link (working back from the hand to the base). This scheme 
allows much faster computation than the conventional technique based on the 
Lagragian formulation of the dynamics [Luh 1980b] . 

Resolved motion force control (RMFC) . - RMFC is a position control method 
developed for robot manipulators [Wu 1982] . Rather than controlling the joint 
positions and torques (as do classical control techniques), RMFC controls Car- 
tesian positions and forces. This method has the advantage of automatically 
compensating for the changing configuration of the manipulator, gravity forces, 
and the internal friction of the manipulator. Another advantage of RMFC is 
that the control method can be extended to more than six degrees of freedom 
without increasing the computational complexity. 
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To effect control of a position manipulator, it is usually desired to 
drive it to follow a given trajectory. Paul's method of defining a manipula- 
tor's position and orientation, the relationship between Cartesian forces and 
manipulator joint torques, the specification of a manipulator's position tra- 
jectory, and the RMFC algorithm are discussed below. 


The position and orientation of an n-link manipulator can be described by 
a 4x4 transformation matrix 


n 
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X 

X 

X 

x 

n 

0 

a 
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z 
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0 

0 

0 
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where P = (p x p„ p z ) T is the position and n x , ny, n z , o x , Oy, o z , a x , 
ay and a 2 are the direction cosines defining the manipulator's orientation. 


The matrix T n is the product of all n transformation matrices, A^, 
i = 1 to n, describing the position and orientation of link i relative to link 
i-1 


T n - Ai A 2 . . . A n 

The linear and angular velocities of the terminal link are a function of 
the Jacobian and the joint rates 


x = [J]^ 


where 


x 



= linear velocity 
= angular velocity 


The terminal link static forces F n = [F^Fy F z M x M y and the 

static joint torques F t = [f^ f£ ... f n -l f n J” are related 

£t " t J J T Fn 


where 


[J]T is the transpose of the Jacobian; the differential change in posi- 
tion and orientation of T n as a function of all n joint angles 

fl, ... f n are the joint torques; F x , Fy, and F z are the 
Cartesian forces at the end-effector 

M x , My, and M z are the moments at the end-effector 

A time-based position trajectory can be defined as: 

T n (t+ t) =; 
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where v(t) is the linear velocity and w(t) is the angular velocity T n (t), 
v(t), and w(t) must be continuous. 

The basic idea of RMFC is to calculate the joint torques required to con- 
trol the Cartesian motion of the manipulator. The method is based on the rela- 
tionship between the joint torques and the terminal link forces. The control 
is divided into two parts. The first part deals with position control of the 
terminal link. The second part of resolved motion force control calculates the 
joint torques required to obtain the desired terminal link forces and moments 
that were determined in the first part. 

For the first part of the control it is assumed that all links except the 
terminal link and the load are massless. Also, the center of mass of the load 
is located at the origin of the terminal link coordinate system, and the coor- 
dinate axes of the load are aligned with the terminal link axes. Because the 
manipulator's links are not massless, the second part of the RMFC scheme com- 
pensates for these errors .using a method called force convergent control. The 
technique uses the Robbins Monro stochastic approximation method to determine 
the joint torques required to produce the desired Cartesian forces and moments 
of the center of mass of the load. 

The desired position and orientation can be expressed in the form of a 
transformation matrix T n( j. The actual position and orientation T na is 
determined according to equation for T n above. The Cartesian position and 
orientation error 


x e x d “ x a f d x ^y ^z ^ x - y ^z^ T 


can be written as 



The differential transformation matrix is obtained as follows. The trans- 
formation matrices Tx, Ty, and Tz defining a rotation about the x, y and z 
axes, respectively are given by 
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For a differential change d0j 


lim sin0=d0 
0-K) 

lim cos0=l 
0-K) 

Substituting these expressions into T x , T y , and T z , multiplying the 
resulting matrices together, and neglecting second order terms gives 



Multiplying T (on the left or right) by a matrix T d defining a differen- 
tial translation along the x, y, and z axes gives 
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This is precisely the differential transformation T n( j* It should be noted 
that this matrix is independent of the order of rotations. 

Explicit expressions for d x , dy, d z , 6 X , 6y, an< * S z of Tnd 
are obtained as follows. 



Rewriting gives 



Premultiplying T nc j by and equating elements of the resulting 
matrix with the elements of T gives 
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where n d , o d , ®d» Pd> ^a» ®ai 
of T nd and T na , respectively. 

The actual Cartesian velocity 

T 

X — [V V V w w wl 
-a 1 xa ys za xa ya za J 

is given by 


p a are the column vectors 


\ 

at T 

n 



where [J] is the Jacobian of the manipulator and q a are the actual joint 
velocities. 


Rewriting the equation for T n (t+At) gives an expression for the desired 
Cartesian velocity X d = [V xd v yd v zd w xd w yd w zd ] T : 
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where At is the sample time 

Performing the multiplication on the right-hand side above and equating 
elements of the resulting matrix to the elements of the matrix on the left side 
yields 


*d 


\d (t > 


[n d (t) • [p d (t + At) - P d (t) ] ] /At 

v (t) 


[° d (t) ' [p d (t + At) - p d (t) ] ] /At 



[ a d ( t ) • [p d (t + At) - P d (t)]]/At 

“ S d (t) 


[a d (t) * o d (t + At) ] /At 

“yd <C) 


[n d (t) ‘ a d (t + At) ] /At 

><« 


[o d (t) * n d (t + At) ] /At 
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The Cartesian velocity error can be calculated by subtracting 

X= -X - X 

— e — d —a 

i 

The desired Cartesian acceleration can then be estimated 
X^(t) = (x d (t + At) - X d (t))/At 

The position control loop for the system uses position and derivative 
feedback in addition to the desired acceleration. The actual acceleration is 
given by the expression 

x a (t) = K v x e (t) + KpX e (t) + x d (t) 

where : 


K p is the position gain K v is the velocity gain. 

Differentiating the equation for Cartesian error and substituting the 
result into the above gives 

X (t) + K X (t) + K X (t) = 0 
e ve pe 

To drive X a to converge to X d , K v and Kp must be chosen so the 
roots of this equation have negative real parts. 

i 

The desired Cartesian forces and moments can be calculated asj = 

! 

where 
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m t is the total mass of the load 

I x , Iy, I z are the second moments of inertia about the coordinate 
axes of the load. 

The applied joint torques required to produce the desired Cartesian forces 
and torques can be calculated 

F t = U] T F d 
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When the mass of the terminal link and the load approaches the mass of the 
remaining portion of the manipulator, the control algorithm will not converge 
to the desired Cartesian forces and torques. Force convergent control compen- 
sates for this error. 


Force convergent control uses Robbins-Monro stochastic approximation to 
calculate the applied Cartesian force F^ (and the corresponding joint tor- 
ques) required to drive the observed Cartesian force F^, to converge to the 
desired Cartesian force F^ . 


The Robbins-Monro method, which finds the root of a regression function, 
can be summarized as follows. Let be the parameter vector to be estimated 

and the be the observation at time K. Then the new estimate at time K+l 
is given by 


^K+l h. b K^K 


The coefficient is a sequence of positive numbers with the following 
properties 


lim 



b K ‘ 0 

hR * " 




< <* 


The first equation ensures that the estimate will settle down in the lim- 
it. The second ensures that the estimate does not settle down before reaching 
the root. The third ensures that the variance of the accumulated noise is fi- 
nite so the effect of the noise can be corrected [Fukunaga 1972] . 

The manipulator is modeled as an unknown function. The input of the func- 
tion at each sample time is F a , the applied force. The function's output is 
the observed force F 0 at the load. This can be stated as 


F 0 = FCF*) 

The method can be described as three basic steps. First, define an error 
tolerance for the force 

dF = [dFj_ dF 2 dF 3 dF 4 dF 5 dF 6 ] T 

This is the acceptable error between the observed Cartesian force F 0 and the 
desired Cartesian force F^. 

Next, initialize the applied force £ a at t = 0 to the Cartesian force 
caused by the gravity force of the load. Finally, at each sample time, perform 
the following operations: 

1) Calculate F<j using F^(t) = MX a (t) 

2) Set K=0 and £ 3 ( 0 ) = final F a of previous 
sample time; 
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3) Apply the joint torques F t = [j] T F a (K); 

4) Measure the observed force F n (K) ; 

i i 

5) Calculate the Cartesian force error) 6F(K) = - j^OO 

6) Calculate 

a) If > i = 1, 2, . . . , 6, compensate 

F a according to the following equation 

P^(K+1) = Fa(k) + by, 6F(k) 

where bj^ = 1/(K+1) for K = 0, 1, 2, . . . 

b) Increment K by 1, 

c) Return to step 3 (apply new joint torques); 

7) 1 If 1 6F ± | < dF ± ,i = 1, 2, .... 6, F 0 (K) 

has converged to the desired force F^. Therefore, apply the joint 
torques in step 3) for the remaining portion of the sample time. 

It should be noted that in the simulations documented in [Wu 1982], the 
above algorithm was modified to perform a finite number of iterations, N, 
rather than using the force error criterion to test convergence. As N is in- 
creased, the accuracy of the algorithm increases. It should be noted further 
that N cannot exceed A t/h, where At is the sample time (in seconds) and h is 
the time (in seconds) required to perform the operations in FCC. 
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Adaptive Control 


The algorithms discussed so far for manipulator control all rely on the 
assumption that the manipulator dynamic parameters are known for use in control 
law formulation. Although the general form of the manipulator equations is 
well known, the precise coefficients (specifically the inertia matrix) will 
generally be time-varying. One technique to overcome the effects of the time- 
varying manipulator dynamics on system performance is to make the controller 
adaptive. While the terra adaptive has been used loosely in many applications, 
it will be defined in this discussion to mean "parameter adaptive" control. 

A parameter adaptive controller adjusts the coefficients of a variable 
"filter." Figure 32 is a block diagram of an adaptive controller. 



Figure 32. - General adaptive controller. 

As shown, it is assumed that the plant to be controlled is either initial- 
ly unknown or time— varying. The parameter adaptive controller consists of two 
primary blocks — the parameter estimation block and the control law block. 

Parameter estimation . - The parameter estimation block determines, using 
sensor data, a set of coefficients that describes the system to be controlled. 
This definition is purposely vague because a large number of parameter estima- 
tion techniques exist. A typical parameter estimation block is shown in Figure 
33. 


84 





Figure 33. - General parameter estimation. 

If the plant is a linear system, it can be expressed as a discrete model 
as 

Y(K+1) = ajYCK) + ... + a n Y (K-n+1) + b 0 U(K) + ... + b m U (K-m) 

The parameter estimation algorithm operates on past values of input and 
output data to generate an estimate of the unknown parameters - a^, ... a n , 
and bg , ... b m . 

Techniques for parameter estimation have been widely studied both for con- 
trol and signal-processing applications. Algorithms are available for determi- 
nistic and stochastic systems. 

The parameter estimation algorithm to be discussed here is recursive least 
squares (RLS). It is one of the most robust estimation schemes and converges 
more rapidly than some of the simpler methods. Recursive least squares is 
classified as an "equation error" parameter estimation scheme because of the 
basic structure of the algorithm. Most parameter estimation techniques are of 
the form [Goodwin 1982] 

§(K+1) = |(K) + r e(K) 

In "equation error" estimation schemes, the error is defined as 
e(K) = Y(K) - IIO(K) 

where 

1= [Y(K— 1), . . . , Y( K-n+1 ) , U(K-l), ... ,U(K-m-l)] T 
0 = [a^,...a n , bg,...b m ]'^ 
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4 


\ 


For the true parameters, e(K) = 0. Thus the name "equation error." e(K) 
is a measure of how well the parameter estimates satisfy the governing equa- 
tion. If e(K) £ 0, the parameter estimator will continue to update the esti- 
mate. The "r" term is what differentiates parameter estimation schemes. It 
can be thought of as a "gain" term for the estimation process. For the recur- 
sive least squares estimator, is chosen to be [Goodwin 1982] and [Franklin 
1981] 


_ a(k) P(K-d-l) (j)(K-d) 

1 + a(K) $(K-d) T P(K-d-l) <j)(K-d) 

and 

a(K) P(K-d-l) (j)(K-d) l{KK-d) T P(K-d-l) 

P(K-d) = P(K-d-l) 

1 + a(K) $(K-d) T P(K-d-l) <f>(K-d) 

To start the RLS algorithm, 

P( -d-1) = e Q I 

where e 1 and 

I is the identity matrix 

0 < a(K) < 2 

f(0) = [0, . . ,0, 1,0, . . .0] 

where the "1" corresponds to the parameter bg. 

If a good "first guess" of the parameters is available, it can be used for 
|(0). The estimate of the coefficient bg should not be initialized to zero 
because this results in division of 0. 

The coefficient a(K) is usually chosen to be unity; however, in cases 
where <j)(K-d) T P(K-d-l) ^>(K-d) ( in the denominator of Y ) is close to minus 1, 
a(K) should be chosen to avoid division by a number close to zero. 

Once an estimate of the parameters has been obtained, it is used in for- 
mulation of the control law. The most general control form is model-reference 
control. In this case the controller functions to make the closed-loop system 
match a desired transfer function (or matrix). A special case of model- 
reference control is "one-step ahead optimal" or minimum-variance control. 

These control strategies are discussed in more detail in the next section. 

Linear Adaptive Control . - A linear, deterministic system is described by 
an auto-regressive moving average (ARMA) equation of the form 

Y(K+1) = a] Y(K) + ... + a n Y(K-n+l) + bg U(K) + ... + b ra U(K-m) 
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To formulate a "one step-ahead" or "deadbeat" controller, it is assumed 
that the reference signal is known one step in advance. That is, Y*(K+1) must 
be known at the time K. The control law for the parameter adaptive controller 
is obtained by setting Y(K+1) = Y*(K+1) and solving for U(K) [Goodwin 1983]. 

This yields 

U(K) = 1_[ - ai Y(K) - ... - a n Y ( K-n+ 1 ) - b 1 U(K-l) 
b 0 - ... -b m U(K-m) + Y*(K+1) ] 

This can also be written in vector notation as 

U(K) = |(K) t 0(K) 

where £(K) = [ -Y(K) ... -Y(K-n+l) - U(K-l)... 

-U(K-m) Y*(K+1 )] t 

0(K) = fix £n *Ll i_"| 

L b 0 *•• b 0» b 0 **• b 0> b oJ 

The vector $)(K) is referred to as the regression vector and 0(K) is the 
parameter vector. This is referred to as the linear controller form because 
the control law is a linear function of past Y and U values. If the a and b 
coefficients are known, the control for U(K) will result in Y(K+1) = Y*(K+1). 
Because it is assumed, however, that the system parameters are initially un- 
known (or time-varying in the case of manipulator control), the a and b co- 
efficients must be estimated. The control law then becomes 

U(K) = 4>(K) t f(K-) 

where 0(K) is the vector of parameter estimates. 


A, 

The parameter estimation algorithm used to obtain 0(K) is RLS as explained 
in the previous section. 

While this form of control is appealing from the standpoint of its simpli- 
city and apparent tracking performance, it is impractical for implementation in 
an actual system. The superior performance is attained at the expense of large 
control efforts. For practical systems this leads to saturation of control 
components and hence full performance is never achieved. 

A more reasonable approach to the problem is to force the closed-loop sys- 
tem to behave as some reference model that is chosen with the limitations of 
amplifiers and actuators in mind. (Strictly speaking, the one- step-ahead con- 
troller is a model reference controller. The reference model is simply a pure 
delay block with the number of delays governed by the number of delays in the 
plant ARMA equation.) 
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The first step in the formulation of a model reference controller is to 
specify the structure of the reference model 

Y*(k+1) = ei Y*(k) + .. + e n Y* (k-n+ 1 ) 

+ hgR(k) + . . . + hj Q R(k-m) 

In this case, the "e" coefficients govern the poles of the reference model, the 
"h" coefficients govern the zeros, and the sequence R(k) is the reference in- 
put signal. 

The first step in the formulation of the model reference control law is to 
isolate the current input term, U(k) as before 

U(k) = 1 [-a^Y(k) - ... - anYCk-n+l) - b^Ck-l) 

b 0 

-b m U(k-m) + Y*(k+1)] 

This step was sufficient to generate the control signal for the one-step- 
ahead controller. Now, instead, Y*(k+1) is replaced by the terms on the right- 
hand side of the reference model 

U(k) = 1 (-a^Y(k) - .. a n Y(k-n+l )-b ]U(k- 1 )~b ra U (k-m) 

b 0 

+e 1 Y*(k)+ ... +e n Y*(k-n+l) 

+h 0 r(k) + ... + hjjjRCk-m)] 

This control law could be used as is by keeping track of previous Y* val- 
ues. However, assume that Yj and not is used in generating the con- 

trol. Substituting and combining terms yields 

U(k) = _l(ej_-a]_)Y(k) + .. + (e n -a n )Y(k-n+l) 
b 0 

-b-^U(k-l) - .. -b ra U(k-m)+hoR(k) 

+ . . . + hjjjRCk-m) ] 

Under these conditions, this system has the compensation structure shown 
in Figure 34. 



Figure 34. -Model reference control structure. 
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When the control law above is substituted into the original system equa- 
tion, this results in: 

U(k+1) = e]Y(k) + .. +e n Y(k-n+l) 

+hQR(k) + .. +h m R(k-m)] 

Subtracting the above from the basic reference model equation and defining 
E(k) = Y*(k) - Y(k) yields 

E(k+1) = e^E(k) + ... +e n (E(k-n+l) 

The significance is that initial startup mismatches between Y* and Y will 
be regulated out as a function of the poles of the reference model. The 
closed-loop transfer function, utilizing this control law, is the desired re- 
ference model transfer function. (This can be shown by simple block diagram 
reduction of Figure 34. ) 

Some simulation results for a single-input, single-output linear system 
are shown in Figure 35. THe performance of the controller when no load change 
is involved is shown in Figure 35(a). Parameter convergence is rapid and 
tracking excellent after initial transient behavior. Figure 35(b) shows the 
results of a load change at 50 iterations. As expected, control degrades as 



(a) No Load Change (b) Load Change at 50 Iterations 

Ljigure 35. - Linear adaptive control simulation. 


Nonlinear adaptive control . - The major body of results developed for 
adaptive control systems have focused on linear systems. For several years, 
however, researchers have addressed the problem of applying the adaptive con- 
trol theory to robotic manipulators. Aside from purely theoretical interest, 
this pursuit is practical. With the increased interest in intelligent, flex- 
ible automation systems, manipulators are expected to be forced to perform over 
their entire load range, often with loads that have poorly defined mass proper- 
ties. An adaptive strategy is ideally suited for this type of situation. The 
problem lies in extending a body of theory developed primarily for single-input 
single-output linear systems to the highly coupled, strongly nonlinear dynamics 
that characterize a manipulator. 

Two of the primary approaches to adaptive control of manipulator systems 
will be described in greater detail. In [Leininger 1983] , the problem is at- 
tacked by assuming that each joint can be adequately modeled as 

(1 + A(Z - 1 ) )Y (t) = Z“ k B(Z _1 )U(t) + e(t) + d(t). 

A and B represent polynomials in Z~ e(t) is a white noise sequence, and 
d(t) is a "takeup" term that accounts for coupling from other joints. A re- 
cursive least squares identification technique is used to identify the para- 
meters of this model. The controller cancels the effect of d(t), (essentially 
a feedforward term), and then computes compensation terms to yield a critical- 
ly damped joint response. Simulations of this approach in [Leininger 1983] 
demonstrate good system response in terms of output tracking, but time hist- 
ories of parameter identification are not provided. Because d(t) represents 
the total contribution of effects from all other joints, it would seem that the 
identification loop would have to be very fast to adequately track this term. 

[Lee 1982] proposed another approach. Rather than approximating the cou- 
pling effects at each joint, the full set of nonlinear dynamic equations are 
used to generate a state-space model that consists of the nonlinear equations 
linearized about a nominal path. An RLS identification routine is used to 
identify the coefficients of the linearized model. Once the state-space form 
is available, a variety of approaches to control exist, including pole- 
placement and optimal regulator theory. Although not referenced, this approach 
to adaptive control is reminiscent of that proposed in [Kreisselmeier 1981] for 
linear systems. 

The nonlinear adaptive control approach that has served as the focus of 
this study was developed by Dr. Howard Elliott at the University of Massachu- 
setts, Amherst. The control approach can be summarized as feedforward cancel- 
lation of nonlinearities at each joint, with subsequent model reference control 
of the remaining linear terms. This approach to nonlinear control is not new 
and apparently was first used in [Heraami 1982]. The significant contributions 
made by Dr. Elliott are: 

1) Recursive least squares can be applied to the nonlinear manipulator 
dynamics without first linearizing the system of equations. 
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2) By assuming knowledge of primitive dynamic parameters, i.e., link 

lengths and the gravity coefficient g, there is a large amount of re- 
dundancy in the manipulator equations and hence the parameter estima- 
tion problem can be drastically reduced. In the case of a three-link 
system, Elliott's back-substitution technique reduces the parameter 
estimation problem from 23 parameters to three parameters per joint, 
for a total of nine. Other studies have shown that the estimation of 
three parameters can be accomplished at 100 Hz with an Intel 8086. 

The use of recursive least squares to directly identify nonlinear terms 
was suggested in [Goodwin 1982]. As an example of this technique, consider the 
nonlinear plant model shown below, where g(Y(k)) is a nonlinear function of 
Y(k), and the structure of the nonlinear function is known, e.g., g(Y(k)) = 
cos(Y(k)) 

Y(k+1) = a 0 Y(k) + .. + a n Y(k-n+l) + a n+ ig(Y(k)) 

+b 0 U(k) + .. + b m U(k-m) 

This can be written in terms of a parameter and regression vector 
Y (k+1) = (jjTe 

— t a l» •••» a n+l » ^0> •••» frmJ • 

Using this parameterization, the recursive least squares technique is directly 
applicable. 

The back-substitution technique for reducing the magnitude of the para- 
meter estimation is described in the following subsection for the three-degree- 
of-freedom planar arm system. This discussion includes the control law formu- 
lation for model reference control and nonlinearity feedforward cancellation. 

Parameter Estimation . - Our starting point is the equations for the 
three-link planar arm. These can be rewritten for the purposes of parameter 
estimation as shown. 

Row 1 


Row 1 

D 1 1 *0 i + D 12 ®2 + D 1 3 ® 3 + d 14 
+ D25 ^cos (02 + 03)(20i + 0 2 ) — sin(0 2 + 0 3 ) ( 0 1 + 02 + 2(0^02 + 0 2 ©3 + ® 1 ® 3 ) )J 
+ £ Jcos0 3 (20 j + 20 2 4- ©3) — sin0 3(0 3 + 20^03 + 2 0 2 0 3 ) -4- D]^70^ 

+ D^gsin©! + Di gsin(0 ^ + 0 2 ) + Dj ^gsin(0^ + 0 2 + 63) = T^ 1 


c 


cos0 2 C 20 1 + 0 2 ) - sin 0 2 (02 + 20 


010 2 ) 


-n 
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Row 2 


D 2l[®'l + + D 22 e 3 + d 23 [cos8 2 (0i) - sin0 2 (0i)J 

+ D 2 4 cos( 0 2 + 63) (0i.) - sin(0 2 4 - 63) ( © 1 )|J 

+ D 2 5 cos03(20i + 20 2 4 83 ) - sin03(03 + 20 j0 3 + 20 2 © 3 )] + 1)2602 
4 D 2 7 Sin( 0 1 4 0 2 ) + D 28 sin(0i 4 0 2 4 0 3 ) = T 2 • 

Row 3 

■03l[®l + ®2 + 03^J + D 3 2 [^cos( 0 2 4- 63 ) C 0 1 ) - sin(0 2 + 63 ) 61 ^] 

4 D 33 |^:os 03 (0 1 4 0 2 ) - sin0 3(03 — © 2 ) 1)3483 
4 D 35 Sin( 0 i + 0 2 + 83 ) = T 3 * 


where 
Row i 

2 2 r 2 2 2 2 
Du = ^l a l C^i + a 2 ) M3 ( L 1 + L 2 + 83) + Ii + I 2 + I3 

2 . 2 - o . 

D 12 = <M 2 a 2 .4 M 3 (L 2 4 a 2' + I 2 ^ 1 3 . 

2 ' 
d 13 “ M 3 a 3 + I 3 

D14 = M 2 Lia 2 + M3 LiL 2 

Dis = M 3 Lia 3 
D i6 = M 3 L 2 a 3 
D 1 7 = B 1 

D 18 = M l a lg + M 2 Lig + M3L1 g 

Dig = M 2 a 2 g + M3L 2 g 
D I 1 0 = M 3 a 38 ’ 

9 
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Row 2 


d 2 1 

= 

2 2 2 

^2 a 2 + M3 (L2 + a3) + I2 + I3 

d 22 

= 

2 

M3 a 3 + 1 3 

d 2 3 

= 

M2L>ia2 + M3L1L2 

D 24 

= 

■ M 3 L 1 a 3 

d 25 

= 

M 3 L 2 a 3 

d 26 

= 

Bl 

D 27 

= 

;M 2 a 2g + M 3 L 2 g 

d 28 

= 

M3 a 3g 

Row 

3 


d 3 1 

= 

2 

M 3 a 3 + ^3 

°32 

= 

;M 3L 1 a 3 ■ 

d 33 

= 

M 3^2 a 3 

°34 

= 

b 3 

d 3 5 

= 

M3a 3 g • 


■_ 

- 


If we assume knowledge of Lj_, L2 and g, we can rewrite these equa- 
tions as — 


Row 1 

D 11®1 + d 12®2 + f* 1 3 9 3 + 


d 14 

»15 


••2 


■3 


Lj (cos02 ( 2 0 ^ • + ©2) ~ sin0£ (©2 gsin(0i + 62) 

p •• •• *2*2 * 

(cos(0£ + ©3) (20 1 + 0£ + ©3) - sin(0£ + 63) (03 + 02 d" 2(0^02 + 6163 H 

..2 


+ L2(cos03(20j + 202 + 63) - sin03(©3 4 - 20^03 + 20263)) 
+ gsiri(0i + ©2 + © 3 )J 
+ Di 70 i + DjsCgsinO^) = Tj • 


0203))) 
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D 21 


[P 1 + 


*.[l 


.2 


+ £>22® 3 + £>23 L l( cos ®2(®l) + sir^CQi) + gsin(0! + 


® 2 )] 


+ I>24 j^l (cos (02 + Q 3 ) (© i ) + sin (0 2 + 63 ) ( 6 i)) + gsin(0^ + ©2 + 63 ) + 
. .L2( c os 3 (20 1 + 20*2 + 63) - sin 0 3 ( 0 3 + 20 1 e . 3 + 20 3 0 2 ))j 
+ D 26 ®2 = T 2 


Row 3 

D 3 l [e> 


+ 00 + 0 




+ D 3 2fL 1 (cos(02 + 0 3 )(*0 1 ) + sin(0 2 + 0 3 )9 3 ) + gsin^ 4- 0 2 + 0 3 ) 
U. .... . 2 . -| 

+ L 2 (cos0 3 (0\ + * 0 * 2 ) + sin0 3 (0 1 + 20i9 2 + 0 2 )) 

+ D 34 0 3 = T 3 • - 


where 

1 

£>14 = M 2 a2 + M3L2 
D15 = M 3 a 3 

£>18 = M l a l + M2L1 + M3 L 1 


D 23 = M 2 a2 + M3L2 
£>24 = M 3 a 3 

D32 = ^3 a 3 


This reduces the number of parameters from 10, 8 , and 5 for rows 1, 2, 
and 3 to 7, 5, and 3. Parameter estimation of 7, 5, and 3 parameters is 
very reasonable and could be implemented practically. However, it is 
possible to further simplify the size of the parameter estimation prob- 
lems for rows 1 and 2 by back-substituting estimates from rows 3 and 2. 


Notice that 



Thus the row 2 equation can be written as 


D 2 i|Si + 0^j + D23 |li (cos©2(0 1) + sin0 2 (0i) + gsin(©i + 0 2 )J + E>26®2 

= T 2 - 63 1© 3 - 632 Li(cos( 02 + 63) ( 6 1 ) + sin(0 2 + 03)(0i)) t 

•• • •• •• *2 • • 
t+ L 2 (cos 03(20^ + 202 + 63) - sin03(03 + 20^03 + 2©2©3) 

+ gsin( 0 i + 0 2 + 03) 


where 


D32 = estimate of D31 
632 = estimate of D32 


O' 


Similarly by observing 

I D 12 = d 21 

D 1 3 = d 31 
D 1 5 = d 32 


We can rewrite the equation for row 1 as 

1 D 1 1®1 + d 17®1 + Dl8|g sin0 3j = T 1 ~ D31 0 3 ~ D21 0 2 


- D23 Li(cos02(20i + 0 2 ) - sin0 2 (0 2 + 20 j 0 2 )) + gsin(0i + 


- D32 1 


0 2 )] 


Li(cos( 0 2 + 63) (26 1 4- 02 + ©3) 

- sin (02 + 03)(03 + 02 + 2(0^0 2 + ©i©3 + 02®3)) 


+ L 2 (cos03(20i + 202 + "©3) - sin03(03 + 20^03 + 20203)) 
+ gsin(03 + 0 2 + 63)^ • 

Each of these equations is now in the form 


-T *- 

XT 0 . = Y. • 

“1 1 1 , 
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where 

s 

X . and Y . 

— 1 1 

contain measurable signals and I © is a vector of parameters to be estimated, 
i.e. , 

© 1 = [pi 1 » d 17> £>18] 

0 2 = [ d 21» d 23» d 26^] 

' 0 3 = [ D 31> d 32. D 3 4~| 

To generate X and Y, we approximate jiQ'i and |i 0 ^ as 

0 .. = e.(kh) = 9.((k + l)h) - (kh) +Q ; ’.((k - l)h) 

ik l l ii 



Notice that one added benefit of this approach is that now all parameters in 
Q i are of the same order of magnitude. This helps the performance of sequen- 
tial least squares. 

Model matching leontrol . - Assume, based on the equations for the three-link 
planar arm, the following discretized model of the planar manipulator 


J(0^) + BV^ + = T^ h = sample period 



V ik ~ li ^®ik " 9 ik - 1^ B “ diag ^ D 17» °26» D 34) 
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J(0. ) 

' '.k 


' D U + 2D 14 cos6 2k \ /D 12 + D 14 cos6 2k V/D 13 

+ 2D 15 cos(6 2k + 8 3 k )j | + Di 5 cos(e 2k + 0 3k > j| + D 16 cos0 3k 


+ 2D 16 cos0 3 , 


+ 2D. 16 cos6 3k 


D 2 i + D 23 cos0 2k \ /D 21 

+ D 24 cos(6 2k + Q 3k ) | | + 2D 25 cosQ 3k 
+ 2D 2 5Cos0 3 

; K 

°31 

+ D 32 cos(0 2 + 0 3k ) 

+ D 33 cos0 3 




' N k“ 


N l, 


r Nj 

N 


2 k 
I No. ! 


= - D 14 jsin0 2k (V 2k + 2v l k v 2 k )] - D 15 [sin(0 2k + 6 3 k H v l k + v 2 k < + 

. ; '< V 4 V -k + V 2k V ?k + V lk V 3 k >] * D 16 [ si " e 3 k <V3 k + 2V lk V 3k + 2V 2 
«2 k * -D23[si“ 9 2 k ( v l k >J - D 24rsi<l( 9 2 k + 9 3 k > v l k J '■ 


+ d 25 


- sin0 3 ^(V 3 ^ + 2(V lk V 3k + V 2v V 3t )) 


3 k'’ 3 k 
2 


: k k 


N 3 k = -D 32 [sin(0 2k + e 3k ) Vi k J - D 33 |sin0 3l (V lk - V 2k > 2 J* 


Assume we want the closed-loop system to be characterized by the model 


E n e ik + l + E i 20 ik + E ^ 0 ik - l = H n r ik + H i2 r i k - l 

E 21 6 2 k + x + E 2 202 k + E 23 0 2k _ 1 = H 21 T 2k + H 22 r 2k _ j 

E 31 0 2 k + i + E 3202 k + E 33 0 2k _ 1 = H 31 r 3k + H 32 r 3k _ l 



An appropriate control is simply 


T k ■ \ + BV ic * J <V\ 

' "1 1 — 1 



An adaptive version of this fixed control is obtained by generating estimates 
of 

J(e, ) , B, and N 
k 

using the estimates of ^ and the equations relating! ‘[©^ to J, B and N derived 
earlier. 

Simulation results for a three-degree-of- freedom planar arm system are 
shown in Figure 36(a) thru (c). For each joint, both tracking performance and 
parameter estimation results are shown 

A full implementation of the adaptive control strategy described for a 
6- DOF or 7-DOF system would represent a significant computational burden. 

There are, however, many ways to reduce the overall complexity of the prob- 
lem. The most obvious approach for reducing the severity of the problem is to 
decrease the complexity of the underlying dynamic equations by ignoring cer- 
tain terms, specifically centrifugal and coriolis acceleration. A second ap- 
proach to making the problem more tractable is to treat the manipulator as two 
decoupled systems. Both approaches will be investigated as future research 
topics. 
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Force/Torque Control 


The previous sections have discussed manipulator control in the context of 
pure positional control. The topic of this section is the control for tasks 
that involve satisfying both positional and force/ torque constraints. For the 
remainder of the section, this type of control will be referred to as "force/ 
torque" control. 

Perhaps the most common example of a task that requires simultaneous sat- 
isfaction of position and force/ torque constraints is the insertion of a peg 
in a hole. This example will be used to introduce a convention for describing 
tasks that was suggested in [Mason 1981], The peg-in-the-hole physical model 
is shown in Figure 37. 


Natural Constraints 



Figure 37. - Compliant control for peg insertion. 

Using Mason's convention, a task is described in terms of "natural" and 
"artificial" constraints specified in a coordinate frame associated with the 
task. Natural constraints are those associated with the task physical geo- 
metry. In the case of the peg-in-the-hole, for example, motion in the X and Y 
axes appears as a natural constraint because it is constrained by the sides of 
the hole. Similarly, forces and torques about the Z-axis appear as natural 
constraints (assuming frictionless surfaces between the peg and hole). Arti- 
ficial constraints essentially determine how the task will be performed. Again 
referring to the example, the artificial constraints include centering of the 
peg (X and Y axes forces and torques equal to zero), and motion of the peg 
along the Z-axis (V^ = ). 
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The two fundamental approaches to force/ torque control are generally re- 
ferred to as passive and active compliance. Passively compliant techniques 
rely on the use of a mechanically compliant device located near the end- 
effector. Misalignments at the workpiece are compensated for by the compliant 
action of this device. The basic idea is shown in Figure 38 taken from 
[Whitney 1982]. While passive compliance can be used for many tasks, the ap- 
proach has certain drawbacks. The most significant is the mechanical resonance 
introduced by the passive device. 



Figure 38. - Rigid peg supported compliantly by lateral 
spring K x and angular spring at distance Lg from peg's tip. 

The alternative to passively compliant devices is an active compliance 
strategy. In an active compliance approach the forces and torques at the 
end-effector are measured and this information is used in a feedback loop to 
provide servo force control. 

The forces and torques at the end-effector are related to joint torques by 
the Jacobian transpose 

T = (J] T F 

where 

T = Joint torques 

F = End effector forces and torques 

[j] - Jacobian matrix 
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This relationship is used to map force error commands from an external 
reference frame to the manipulator joints. It also demonstrates two alterna- 
tives for measuring end-effector forces and torques. First, joint torques can 
be used to reconstruct end-effector forces and torques via the Jacobian rela- 
tionship. Joint torques can be measured in a variety of ways. First, they can 
be measured directly via strain gage measurement devices [Paul 1981a]. In an- 
other approach, joint torques can be derived from joint motor currents through 
the motor torque constants. 

A second approach to generating measurements of end-effector forces and 
torques is to use a wrist-mounted sensing device. While these devices are be- 
coming more readily available from a number of manufacturers, the most popular 
implementation is shown in Figure 39. This is generally referred to as a 
"Scheinman wrist", (after its inventor), and uses 16 strain gages to resolve 
forces and torques. Strain-gage-type force-sensing devices typically have a 
force resolution of 3 to 5 oz. 



Figure 39. - Scheinman force-sensing wrist. 

The force-sensing wrist approach has two key advantages over the joint 
torque measurement approach for determining end-effector forces and torques. 
The first advantage is that the accuracy of the joint torque sensing approach 
is degraded by friction, both viscous and non-linear that appears in all 
manipulators. Also, the derivation of forces and torques is more computa- 
tionally intensive for the joint torque approach [Shiraano 1979]. 

Two approaches to force/ torque control will be discussed. The first 
approach was introduced by Raibert and Craig while at JPL, [Raibert 1981]. 
This strategy involves the use of separate position and force servo loops. 
Errors are generated in a Cartesian reference frame and then mapped into the 
joint reference frame. The response torque at each joint aids in satisfying 
both position and force objectives in the Cartesian space. The approach is 
referred to by the authors as "hybrid" control. The second approach was 
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introduced in [Salisbury 1980], and has been coined "active stiffness" control. 
The controller has the net result of causing the end-effector of the manipula- 
tor to behave as a six-degree-of-freedom spring in space. This is very similar 
to the passively compliant techniques described earlier with the important ex- 
ception that stiffnesses are computer controlled and can be varied as a func- 
tion of the particular task being performed. 


Hybrid Control . - As introduced in the previous section, the hybrid con- 
trol approach is based on the description of a manipulator task in terms of 
position- and force-controlled axes in a task reference coordinate frame. The 
natural constraints partition the degrees of freedom into a position-controlled 
subset and a force-controlled subset. The desired position and force trajec- 
tories are then specified by the artificial constraints. Figure 40 illustrates 
the basic structure of the hybrid controller. This figure will be used to de- 
scribe additional detail of controller functioning. 



Figure 40. - Conceptual organization of hybrid controller. 


Error signals are generated in the task reference frame by comparing de- 
sired trajectories with the actual state of the manipulator. This involves two 
comparisons, as shown in Figure 40. To create the position error (the upper 
loop in Fig. 40), manipulator position in the task coordinate system is derived 
from a kinematic transformation of the manipulator joint angles and subtracted 
from the reference trajectory. A force error is derived in a similar manner 
as shown in the lower loop of Figure 40. This results in a force feedback 
vector that is compared with the desired force trajectory to generate a force 
error vector. 
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The next step in the hybrid control process is "filtering" of the position 
and force error signals by a "compliance selection matrix" [S]. The compliance 
selection matrix is required because the transformations of manipulator posi- 
tion and force variables result in terms that violate the partitioning of the 
task reference frame into orthogonal position and force axes. The compliance 
selection matrix has the basic form: 


Sj_ = 1 if DOF X^ is force 
controlled, = 0 otherwise 


When the force error vector is mapped through the compliance selection ma- 
trix, force components sensed at the manipulator end-effector (in general, com- 
ponents will exist on all six axes) that are not on the -force-controlled DOF 
will be eliminated from the error vector. A similar operation occurs for the 
position-controlled DOF; however in this case the error vector is mapped 
through [I] - [S], where [I] is the identity matrix. 


S = 


Si 

S 2 -0- 
s 3 

S 4 

-0- S5 

S 6 


In the final stage of the hybrid control process, the error vectors for 
both force and position DOF are mapped back into manipulator joint space as 
shown below. 

<U(t) = [J]- 1C Xe(t) 

2_e(c) = [Jj T “Fe(t) 

Position error vector in task coordinate 
Force error vector in task coordinate 
Position error vector in joint coordinate 
Torque error vector in joint coordinate 
Manipulator Jacobian 

The error vectors, q_ and ^t, are then distributed to the individual joint 
servos of the manipulator. A more extensive diagram of the servo control 
structure proposed in [Raibert 1981] is shown in Figure 41. 


Xe( t) : 
Fe( t) : 
9_e ( t ) : 
T e(t) : 

TJ] : 



Figure 41. - Hybrid controller implementation. 
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Active stiffness control . - The concept of active stiffness control, has 
many of the attributes of the hybrid control strategy described earlier, but 
introduces the novel concept of forcing the end-effector of the manipulator to 
behave as a six-dimensional spring in the Cartesian reference frame. As 
pointed out earlier, this bears a striking resemblance to passive compliance 
approaches in terms of accommodating misalignments between the end effector and 
task. Again, the important difference is that in ah active compliance ap- 
proach, stiffnesses on specific axes are controlled externally and varied 
during different phases of a task. For example, during rapid slewing motions 
in moving from point to point, stiffnesses can be set high to minimize reac- 
tions to acceleration- induced forces from the load or tool. 

To arrive at an equivalent six-dimensional spring relationship expressed 
in terms of joint displacements and joint torques 

T = [J] T K[J]<$0 

The matrix = J T KJ is referred to as the joint stiffness matrix. The 
point in the hand reference frame that serves as the basis for the computation 
of the Jacobian is referred to as the stiffness center. This is the point at 
which pure forces cause only translation and pure torques cause only rotation. 

In its most basic form active stiffness control can be implemented by gen- 
erating joint torque commands of the form 

IC = K 0 50+ Tg 

The term Tg is a bias torque generated from the relationship Tg = J t Fr. 

Fg represents bias forces in the Cartesian reference frame. Note that the 
appropriate choice of K leads to a decomposition of force- and position- 
controlled axes as in the hybrid control scheme. 

A six-dimensional linear spring is described by the relationship 

F = [K]6X 

6X is a generalized displacement from a nominal position Xq of the hand ori- 
gin and consists of three orthogonal translation components and three small ro- 
tations about orthogonal axes. This diagonal matrix [K] contains the spring 
coefficients for each axis. 

The Cartesian displacement .<5 X can be related to a manipulator joint dis- 
placement 69, (66^ = ~ Off) through the Jacobian matrix 

6 X = [J]69 

Joint torques are also related to end-effector forces and torques as a 
function of the Jacobian 

T = [J] t F 
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These expressions can be combined to obtain the stiffness matrix Kq. 

In actual implementation of active stiffness control, additional compensa- 
tion elements were required [Salisbury 1980]. The total torque applied to the 
i c h joint is given by 

T,- = Tc,i + GiSTi + Kv,iC(( i6Qi -f- V 0l sgn{Q t } -j- C/ ,- 

\ 

where: 

Tc,i ^commanded torque, rth joint 

6Ti = torque error, ilh joint 

6, ^velocity, ith joint 

(50,- =veiocity error, rth joint 

G,=torque compensation function, ith joint 

Kv.i ^velocity damping term, i lb joint 

Cu i ^instantaneous inertia, ith joint • 

Ct t i —gravity loading, ith joint 
V 0 =friction torque, ith joint. 


A block diagram of the full control loop is shown in Figure 44. 



Figure 44. - Stiffness control system. 

The algorithm implemented within ROBSIM deviates slightly from that just 
presented. The first step in this algorithm is the calculation of the end- 
effector position error ^ P^. This error vector and any prescribed bias forces 

F r are used to compute joint control torques 

\ 

T d = [J] T [K] P + [j] t f b 

where [j] and [K] have the same definitions as stated earlier. A torque error 
vector ,A|T is then determined from 

AT = T c - U3 T F 
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where F is the vector of measured end-effector forces and torques. 

The error vector T is then calculated using 
1 = K LL (-§t #)AT + Ji_AT + T c - KdS0 

s 

where 

Kj^l = lead- lag filter gain 
Kj = integrator gain 
K(j = derivative gain 

Figure 45 shows a diagram of the control loop implemented in R0BS1M to simulate 
active stiffness control. 



Figure 45. - Stiffness control system implemented in R0BS1M. 


The results for an active compliance control simulation are shown for a 
single axis in Figure 46. In this case, the manipulator has been commanded to 
a position that is past the boundary of a rigid constraint. The force pro- 
file shown is the result of the complying action. 
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MANIPULATOR TRAJECTORY PLANNING 


Manipulator systems often must operate in environments "cluttered" with 
obstacles, e.g., the objects being manipulated and the machinery with which the 
arms are interfacing. When performing tasks, the motions of the arms must be 
carefully planned to avoid collisions between the manipulator links and these 
obstacles. Collisions between the various parts of the manipulator system must 
also be prevented, especially in multiple-arm systems of when manipulating 
bulky objects. Manipulator path-planning can also be used to improve task per- 
formance by finding paths that minimize energy consumption or operation time, 
or that avoid singular configurations of the arm, etc. 

To date, path-planning research has focused on two application areas — 
paths for polyhedral objects and paths for robotic manipulators. Polyhedral 
objects are self-contained — mobile objects such as autonomous robotic ve- 
hicles,* aircraft, etc. The obvious difference between such an object and a 
manipulator is that the latter is not "mobile" in the sense that one end is 
fixed to a platform or base that may be mobile. A manipulator arm is a com- 
bination of connected links, each of which is a polyhedron. The "motion" of a 
manipulator is constrained by the link connectivity, which limits its overall 
reach. The volume (or area) reachable by the hand or tool is a manipulator's 
working envelope. When a manipulator moves, each of its links sweeps out the 
path of a polyhedron. Thus, path planning for the motion of polyhedral objects 
is a subproblem in trajectory planning for manipulators. 

The problem of planning paths for polyhedrals has received much attention, 
with the intent of generalizing methods to manipulator path planning. Unfor- 
tunately, the two problems have inherent differences, limiting the application 
of polyhedral methods to the manipulator case. There have been implementations 
of manipulator path planners relying solely on relations among polyhedral ob- 
jects and the polyhedral components of the arm [Brooks 1983b] but these neither 
capitalize on the serial nature of the manipulator arm nor provide for diverse 
manipulator operations. They have also demonstrated success only in very sim- 
ple environments composed of rectangular polyhedrals. 

This section of the report discusses the current state of path-planner 
technology, with the emphasis on path planning for robotic manipulators. The 
first part consists of the history and development of path-planning methods for 
both mobile objects and manipulators. The next subsection examines current 
technology and describes the results of path-planning research under the ROBSIM 
contract. Finally, the third part compares the current methods and provides 
some topics for further study. The emphasis in this work is on path planning 
to avoid obstacles in the manipulator's operating environment. Table IV sum- 
marizes the notation employed in this section. 


* SRI's Shakey and the Mars rover are two examples of such vehicles. 
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TABLE IV. - NOTATION FOR MANIPULATOR PATH PLANNING SECTION 


d 

distance between link and obstacle 

u A ] 

Jacobian relating translational motion of point A to joint motions 

Up] 

Jacobian relating hand motion (translational and rotational) to 
joint motions 

n 

unit vector normal to obstacle surface 

£a 

location (world coordinates) of point A 

Alp 

difference between desired and actual position of hand reference 
point 

e 

margin of safety used in preventing collisions 


vector representing difference between desired and actual hand 
orientation 

9 

joint displacements 

A6 

incremental step in joint displacements 
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History and Development of Path— Planning Methods 


Several approaches for solving the path-planning problem for polyhedral 
objects have been applied. These fall into three categories: 

1) Topological search of discretized environments; 

2) Visibility lines and growing obstacles; 

3) Graph search of representations of free space. 

Each of these methods has been demonstrated successfully for two-dimensional 
environments. Some have been generalized to three dimensions with limited suc- 
cess. Each method is discussed in the following paragraphs. 

Graph search of discretized environments . - A discretized environment is 
one in which continuous space has been divided into a set of points, labeled 
either "free" or "full" depending on whether an object coincides with that 
point. Searching such a space involves finding a set of connected points 
("connected" in the sense that each point in the set lies adjacent to both its 
predecessor and successor points in the world) from the start point to the 
goal. There are two basic limitations to this approach. First, both the start 
and goal positions must be points in the discretized space. If they are not, 
some other technique must be used to find a path from the start to a nearby 
point in the space, and from a discrete point to the goal. The second limita- 
tion is that a path may not always be found when one exists. Again this is at- 
tributable to discretization of the space [Fig. 47(b)]. If the resolution of 
the discrete representation is not high enough, some free space may not be at- 
tainable. Also, small obstacles lying between discrete points may be over- 
looked. Thus, a discretized space may not allow generation of collision-free 
paths in all cases. One method for ensuring that objects are not overlooked is 
to determine the resolution necessary for an environment after considering the 
size of its objects. Eliminating the problem of nonattainable free space is 
more difficult. 



(a) Discretized space. (b) Discrete space with 

impossible goal. 

Figure 47 .- Discretization of the environment. 
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Visibility lines and growing obstacles . - A "visibility line" is a path 
from one point to another in continuous space so that, from either point, one 
would be able to "see" the other point along an unobstructed straight line. In 
two dimensions, this approach finds paths using vertices of the objects in the 
world [Fig. 48(a)]. The basic operation loop of this method is: 

1) Propose a straight-line path from current start to current goal; 

2) If no objects interfere with this path, then return with success; 
otherwise, go to step 3); 

3) Generate subgoal locations avoiding the obstacle; 

4) Select a new subgoal point to achieve; 

5) Go to step 1) recursively. 

The algorithm generates a straight-line "visibility" path from one point to an- 
other and checks to see if it is permissible. If it is, the path is taken. 
Otherwise, a path that avoids the obstructing object and attains the goal must 
be found. The naturally recursive form of this approach makes it attractive 
for solving the polyhedral path-planning problem. 




(a) Visibility line paths. (b) Growing objects. 

Figure 48.- Visibility line method of path planning. 


A severe limitation of this approach is that the object being moved 
through the environment is considered as a point. This is not so severe when 
the objects in the environment are many times larger than the moving object. 
However, some other technique must be used if the moving object is of non- 
trivial size in comparison with other objects. One method of ensuring 
co 11 is ion- free paths in this case is to "grow" the object to be moved [Lozano- 
Perez 1979]. This technique uses the maximal size of a moving object in a 
given attitude, a reference point on the object (the point to be moved through 
the space), and a proposed trajectory to generate areas the reference point 
must avoid while in motion [Fig. 48(b)]. The simplest case of obstacle growing 
occurs when the moving obstacle is a circle with the reference point at the 
circle's center. In this case, objects are "grown" by the radius of the cir- 
cle, effectively generating r-coverings of the objects, where r is the circle's 
radius. 
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Graph search of representations of free space . - As asserted previously, 
there are inherent limitations to finding paths in a discrete space. Both the 
recursive visibility search and the discrete space search methods are limited 
by their knowledge of the environment. Neither method performs well in envir- 
onments containing complex objects [Fig. 49(a)]. By modeling the free space 
in an environment, some of these limitations can be avoided while improving the 
nature of the paths generated. The best example of this method is the general- 
ized cone representation [see Brooks 1983a and Figure 49(b)], In this ap- 
proach, the environment description is preprocessed before any path is sought. 
The planner identifies the object sides that are relatively facing each other. 
The space between each such pair is known as a freeway through space. All of 
these freeways are found and analyzed to be sure that other obstacles do not 
intrude on them. The freeways are pared down according to their intersections 
with other objects in the environment. Those that will not accommodate the 
volume of space swept out by the moving object are discarded. The shape of the 
freeways is described as generalized cones. It is assumed that the moving ob- 
ject will be centered on the axis or "spine" of a cone as it moves along it. 

The freeways thus constitute the set of motions available to the moving object. 
The spines of the free space are used in planning a path for the object. As 
indicated in Figure 49(b), this approach generates paths that tend to keep the 
moving object as far from stationary objects as possible. 




(a) Paths generated by discrete space 

(left) and visibility lines (right) 
approaches for a more complete 
object. 



(b) Generalized cones generated by three 
objects and boundary. 

i Figure 49. -Path planning in !, 

complex environment. 
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Path-Planning Research Under the ROBSIM Contract 


Three approaches to manipulator path planning were investigated under the 
ROBSIM contract: 

1) The generalized cone method; 

2) The joint space method; 

3) The incremental, constrained-motion method. 

Research applied to these methods resulted in preliminary implementations of 
each approach. The following is a discussion of each method, its 
implementation, and the outcome of the research. 

The generalized cone method . - A path planner based on the approach deve- 
loped by Brooks (see [Brooks 1983a and 1983b]) and the previous discussion), 
in which free space is represented by generalized cones and trajectories along 
the spines of these freeways are generated, was implemented initially. To 
decrease the amount of computation involved in the algorithm, some simplifi- 
cations are imposed on the planning environment. All objects in the environ- 
ment, including the arm, are assumed to be rectangular in shape and grid- 
aligned (i.e., their sides are parallel to the sides of the workspace). This 
limitation is not overly restrictive. Irregularly shaped objects can be en- 
closed within bounding boxes. Few feasible solutions will be excluded by the 
use of this approximation, especially considering that this planning algorithm 
prefers paths that follow wide lanes rather than paths for which the arm barely 
fits between obstacles. 

Programs for testing the algorithm were also developed. These programs 
generate random obstacles, start points and goal points within a two- 
dimensional workspace. Any number of objects are allowed in the environment. 
Four obstacles provide good testing results; with more obstacles, the computa- 
tion time for preprocessing the environment grows rapidly. This problem is in- 
herent in the Brooks algorithm but might be circumvented by applying heuristics 
to determine which calculations are necessary. 

The planner was quite successful in solving the problems it set for it- 
self. After generating a workspace, it would preprocess the environment, 
establishing the path freeways, and seek a path from the start toward the goal. 
Although it found a path in the majority of cases, the paths were sometimes 
very indirect. This may be caused by some subtle programming bug yet to be un- 
covered. The planner is able to rotate the arm as well as translate it, and 
occasionally finds intricate paths involving sequences of rotations and trans- 
lations through several corners. 

The joint space method . - Joint space is an N-dimensional vector space, 
where N is the number of manipulator joints or degrees of freedom, in which 
each coordinate axis corresponds to one joint displacement. Any point in this 
space represents a unique configuration of the manipulator. Similarly, with 
appropriate bounds on the joint displacements, any configuration of the arm 
corresponds to a unique vector in joint space. If, for any configuration of 
the manipulator, some link collides with an obstacle (including another link), 
that configuration is forbidden. In the joint space method of path planning. 
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all obstacles and position constraints are mapped into forbidden regions in 
joint space. The problem of finding a coll is ion- free path for the manipulator 
therefore reduces to that of finding a point-path (curve) through joint space 
that avoids forbidden regions (Fig. 50). Although the initial configuration 
(starting point) is uniquely defined, the target point is often not unique and 
might even constitute a goal surface. 




(a) Physical space composed of (b) Corresponding angle space 
manipulator and object. representation. 

Figure 50.- Joint space method, .of path planning. 


The attraction of this approach to path planning is that a relatively sim- 
ple (and fast) search algorithm, such as the recursive visibility lines method 
outlined earlier, may be used to find paths for the entire manipulator. This 
method's main drawback is that the translation algorithm for mapping obstacles 
from physical space into their joint space representations can be exceedingly 
complex and computationally time-consuming. This can be attributed not only to 
the complexity of the inverse kinematic solution (i.e., finding joint displace- 
ments for a prescribed tool position), but also to the fact that collisions be- 
tween the obstacle and all points of the manipulator (not just the endpoint) 
must be mapped. Therefore one point on an obstacle maps into one or more ir- 
regularly shaped regions in joint space. 

Another limitation is that search routines such as the visibility lines 
method do not generalize directly to higher dimensions. This is because the 
routine generates paths from vertex to vertex in avoiding an obstacle. In two 
dimensions this can ensure an optimal path; in three-dimensional space it al- 
most never does. This is because in higher dimensions, the shortest path 
around an obstacle usually involves crossing its surfaces at points other than 
vertices. Although methods that discretize three-dimensional edges have been 
used to overcome this problem (see [Lozano-Perez 1979]), these methods suffer 
from the same drawbacks as those discussed earlier. 
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Alternatively, the procedure can readily be modified to find optimal paths 
as described in the following discussion. Rather than using only vertices of 
the obstructing surface, consider instead the edges of the surface (Fig. 51). 
The points (C,C') along this edge that minimize the total distance traveled 
from A to the edge to B are readily evaluated as the solution of a set of lin- 
ear equations. When more than one obstacle face is encountered along the path 
from A to B, successive application of this approach leads to a suboptimal so- 
lution. Alternatively, a modified quadratic programming algorithm can be em- 
ployed to solve for an optimal solution. Hueristic or search methods can be 
applied to determine which obstacle edges to consider. 



] Figure 51.- Visibility line method in three dimensions. 


Another method that holds promise for implementing the joint space method 
is based on a simplified representation of the obstacles. A more simplistic 
representation of the forbidden zones in high-order spaces could be the key to 
generating optimal paths. Representations that break up zones into regions of 
free or full rectangular parallelepipeds (in much the same way as the discrete 
space representation) facilitate the use of very simple interference detection 
algorithms, and could be used to find near-optimal paths in higher dimensional 
spaces [Haralick 1983]. 

Improved methods such as these are being investigated for extending the 
joint space method to higher dimensional spaces. 

A preliminary implementation of the joint space method was developed for 
a manipulator of two joints. For a two-link manipulator with two rotational 
joints, there are two joint angle solutions for any given end-effector position 
in the reachable space. Figure 52 lists the translation functions that deter- 
mine values for the joint angles corresponding to a given end-effector posi- 
tion. None, one, or possibly both of the joint angle solutions may be found 
to cause interference with an object. The complete mapping of an object re- 
sults in a closed curve in the angle space. This curve is approximated ini- 
tially by a large number of line segments. Next a minimal bounding box is gen- 
erated enclosing all these segments (and thus the curve). Finally, an axis- 
oriented bounding box is generated to be used in rough positioning. Figure 53 
shows the resulting hierarchical joint space representation. 
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Figure 52 .- ^Translation functions from physical to angle space. 



Figure 53.'- An hierarchical 
•object representation. 


Once all objects in the physical space have been represented in joint 
space, the path-planning operation begins. The start and goal configurations 
of the manipulator are the start and goal points in joint space. The planner 
determines which "objects" (called "zones" in the joint space) are relevant by 
a simple bounding box intersection check [Fig. 54(a)]. Next, a straight-line 
path from start to goal is proposed. If a forbidden zone interferes with the 
proposed path, subgoal points are generated to allow the planner to avoid the 
offending zone. Thus the search operation of the planner follows the path of 
the recursive visibility lines method (Fig. 54). 

It was thought that an indepth study of the simple two- joint case would 
yield insight into solutions in the general case. However, the generaliz- 
ability of this method to higher degrees of freedom is inhibited in two impor- 
tant areas. First, the closed-form translations of objects in the physical 
world to their joint space representations are difficult to determine, espec- 
ially for manipulators with several degrees of freedom. One method to over- 
come this problem involves the notion of incremental learning of the physical 
environment. This will be discussed in the final subsection. 

The second drawback to the joint space method is that search routines for 
higher order spaces tend to generate nonoptimal solution paths. As described 
earlier, algorithms are being developed to overcome this limitation. 
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a) relavent objects b) proposed path and c) the result ing path 

found via rough check generated subgoals 

Figure 54.- Path planning in joint space. 


Initially, the joint space method for manipulator path planning seems at- 
tractive. Once a static space has been represented in the joint space for a 
given arm, the problem of finding a path for the arm is reduced to a much sim- 
pler and faster dynamic graph search problem. The problems inherent to this 
method provide some genuinely difficult research areas, both in geometric mo- 
deling and graph search techniques. However, for planning in a static environ- 
ment for an arm with three degrees of freedom or less, this method is viable. 
This is particularly true for applications demanding that plans be generated 
in a short amount of time. 

The incremental, constrained motion method, — This subsection describes a 
new approach to manipulator path planning in which the end-effector (or hand) 
moves toward the goal position in incremental steps. At each step, a check 
for potential collisions with obstacles is made. If any exist, the motion at 
each step in constrained to avoid the obstacles, while still moving toward its 
goal. A linear programming algorithm is used in this case to solve for the 
joint motion increments. The constraints and optimization criteria are trans- 
formed to joint space for each instantaneous position of the device. The two- 
dimensional case is implemented and used as a basis for discussion in the 
following paragraphs, although the method extends directly to three dimensions 
(nonplanar operations). 

All obstacles are defined by enclosing line segments. The line segment 
representation is stored as a pair of endpoints and the outward-facing unit 
normal. Assume the manipulator is in an allowable initial or intermediate 
configuration. First, the world coordinate positions of all links and points 
of interest in the arm are evaluated as discussed in the Analysis Tools sec- 
tion of this report. Then, all potential collisions are identified. Figure 55 
illustrates the three types of collisions possible in the planar case. These 
types involve (1) point on manipulator with edge of obstacle, (2) vertex of 
obstacle with link of manipulator, and (3) point on manipulator with vertex of 
obs tacle. 
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Figure 55.- Arm position in cluttered environment. 


User-specified bounding covers (e -covers) are defined around each manipulator 
link and point of interest. Potential collisions are identified by checking 
all manipulator points against all obstacle edges to see if they are within a 
distance E of each other.* Also, all obstacle vertices are checked for prox- 
imity to manipulator links. This checking process involves two steps: (1) 
evaluate the distance between the point and the line containing the edge seg- 
ment; and (2) if this distance is less than E, check that the point is within 
the segment of this line defined by the edge endpoints. Potential collisions 
of type (3) are determined as an intermediate result of this check. 

For each potential collision identified, a corresponding constraint is 
"activated" to prevent the collision from occurring on the subsequent motion 
step. Consider the point/line interaction shown in Figure 56. 


* The efficiency can be improved by limiting the point-edge pairs checked to 
those that lie within the same discrete neighborhood. 
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Figure 56. - Constrained motion of joint! 


Because point A is within distance e,of the line, the constraint is activated. 
The constraint placed on the relative motion of A toward the line is that A 
cannot move closer than a distance £/ 2 from the line. This method is employed 
to prevent A from jumping in and out of thei e-cover, repeatedly activating and 
deactivating the constraint. This constraint can be written 

Ar^ • n > | - d 

where n is the outward-facing unit normal, d is the current distance between 
the point and the line, andAr^ is the incremental motion of point A rela- 
tive to the line for the subsequent step. This constraint is transformed to 
joint space by the relations 

ir A - [J A ] 48 

n T [JjAi if - d 


where is the vector of incremental joint displacements and [J^] is the 
translational Jacobian for point A, which is discussed further in the Analysis 
Tools section. Constraints of type (2) are handled identically, considering 
the moving link fixed and relative motion of the points of the environment. 

For constraints of type (3), the line normal n is replaced in these equations 
by a unit vector directed between the interacting points. 

The incremental motion A £ is therefore constrained by a set of linear in- 
equality constraints. Additionally, limits are placed on the magnitude of the 
change in each joints displacement 

A0 I < AO. 
i' — i,max 
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rr, . 

To determine the joint motion stepA'_0, an error vector (Ar^ A«l> * J T 
between the current hand position and the desired hand posTtion“ rs computed as 
described in a previous section. Then, the equations 


[J p ]A_e = 


r Ar 1 
~P 


Adi 

-P 


J 


are solved, subject to the joint stepsize limits, using the linear equation 
solution algorithm previously discussed. This provides straight-line motion of 
the hand toward the goal. If, however, the resulting joint displacements vio- 
late any of the obstacle-avoidance constraints, the equations are solved by a 
different method. 


In this case, all activated joint motion constraints are considered and 
the simplex algorithm for linear programming is applied. This is analogous to 
the method described for a similar manipulator path-planning problem 
[Grechanovsky 1981]. The linear optimization criterion is defined by 

maximize (Ar • Ar „ + Ad> • Ad> } 

“P — p, actual — p — p, actual 

which is translated into joint coordinates to give 
! maximize {(Ar^, [J^] A8} 


The joint angles are updated using the resulting vector 


0 

—new 


—old 


+ A0 


and the entire process is recursively repeated until the goal is reached. 

Initial testing verified that this algorithm works very well as a low- 
level planner in an overall planning hierarchy. However, the manipulator can 
easily be trapped against the edge of one or more constraints, endlessly cy- 
cling among a few positions without making any progress toward the goal. This 
problem illustrates the need for a high-level planner that provides interme- 
diate goals to the incremental-motion planner and monitors its operation. 


The ROBSIM path planner contains an initial implementation of such a high- 
level planner. The Cartesian position space of the hand is discretized into a 
finite number of regions. Weights are assigned to transitions between conti- 
guous regions; these weights indicate the relative ease of making the hand mo- 
tion corresponding to that transition (small values indicate the transition is 
readily achieved). The high-level planner chooses a goal-directed path, de- 
fined as a sequence of regions to be traversed, based on these weights. The 
center point for the next region along the path is specified as the current 
goal for the low-level planner. The high-level planner then monitors the exe- 
cution of the lower level. 
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If the goal region is attained within a small number of steps at the lower 
level, the weight corresponding to that transition is decreased and the next 
target region is prescribed. If the goal region is not attained within a cer- 
tain number of lower level steps, the weight for that transition is increased 
and the high-level path is resynthesized. In this manner, the high-level rep- 
resentation of obstacles is adaptively learned as the arm executes planning mo- 
tions. This high-level planner prevents the low-level planner from becoming 
entrapped. This hierarchical design has provided good test results although 
the current graph search algorithm in the high-level planner needs modification 
to improve the overall planner performance. 

Other high-level planning approaches in addition to this simplified 
discrete-space method could also prove productive. For example, a rule-based 
planner could be implemented. 

Rules in the high level are simple heuristics pertaining to possible arm 
motions. For instance, a rule to determine a direction to be taken (by the 
manipulator's end-effector) in avoiding an object is 

(can-reach-around fcarm ^obstacle fcgoal-orientation) - 
(propose-path fcarm ^obstacle BEYOND) 

This rule says that if the arm ($arm) can attain its final orientation ($goal- 
orientation) by reaching around the obstacle (^obstacle), then a sequence of 
subgoal configurations should be generated allowing the manipulator to avoid 
the object by moving BEYOND it. A situation in which this rule may be used is 
shown in Figure 57(a). Moving around an obstacle means that the overall length 
of the arm increases, rather than shrinking toward the base of the arm [see 
Figure 57(b)], Another general heuristic says that shrinking the arm is pre- 
ferred to extending it because a smaller arm tends to interfere with fewer ob- 
jects than an extended one. Another rule points out that any object that in- 
terferes with link i may also interfere with links i+1, i+2,..., L (where L is 
the total number of links). The rules of the high-level planning system should 
be simple and their interactions limited to keep the planning speed of the 
overall system high. 




a) manipulator reaches around 
object to attain 90a! 


b) smaller manipulator cannot 
reach around; finds path 
beneath the object 


Figure 57.- Actions of a rule-based planner. 
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The high-level planner does not attempt to find complete collision-free 
paths in any sense. Rather, it guides the operation of the low-level planner 
using information about the current state of the manipulator to generate plaus- 
ible paths. This system may attempt ultimately impossible paths for some time 
before deciding on a new approach. However, when this happens, the system can 
be capable of large "jumps" back to previous manipulator configurations, par- 
ticularly if the planning is completed before the motion begins. This back- 
tracking scheme is maintained by the high-level system. Thus, when an attempt 
to reach a goal orientation becomes less desirable (or fails) in the current 
path direction than in some previous one, the system may return attention to 
the previous path, possibly jumping back over many attained configurations. 


Comparison of Current Methods and Directions for Further Study 


Three methods stand out as viable solutions to the problem of manipulator 
path planning — the generalized cone representation of free space, modeling phy- 
sical environments in joint space, and incremental, constrained-motion plan- 
ning. Each of these methods is directly applicable to manipulator path- 
planning problems. However, depending on the application, one method may be 
more suitable than another. In this section, each of these methods is dis- 
cussed in terms of its strengths and weaknesses in various path-planning do- 
mains. Following this is a brief discussion of directions for further study. 

The generalized cone representation of free space . - Recall that repre- 
senting physical environments as a set of intersecting generalized cones in- 
volves finding cones between relatively facing edges (or faces, in three dimen- 
sions) of all the objects in the environment. The time required to generate 
the representation grows rapidly with the number of objects.* For this reason, 
the method is limited to applications involving only sparsely populated envir- 
onments. However, if objects may be represented in terms of simple geometric 
figures (such as bounding boxes or rectangular parallelepipeds), evaluation of 
the free space becomes more efficient. 

Another limitation of this approach is that solutions are determined sole- 
ly through examination of possible freeway paths. This leads to the same pro- 
blem as the discretizing of an environment; namely, the algorithm may not find 
a path even when one exists. This is because the object moving through the en- 
vironment must remain aligned on a freeway at all times. Additionally, rota- 
tion is allowed only at freeway intersections (i.e., within the intersection of 
two cones). Thus, the planner may fail to find paths that require rotation be- 
tween two intersection points. If the distance between objects in the environ- 
ment is relatively large in comparison with the size of a manipulator's links, 
this is only a minor problem. 


* According to Brooks, the algorithm has complexity (n3) "n cubed" in num- 
ber of objects for 2D worlds; (n4) "n to the fourth" for 3D (see [Brooks 
1983a], pps. 194, 196). 
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Modeling physical environments in joint space . - The most limiting aspect 
of this approach is calculation of the joint space representation of the physi- 
cal world. Because there are many ways for a real manipulator to attain a 
given end-effector position, an a priori determination of all possible arm in- 
terferences is difficult, if not impossible. However, a preliminary determina- 
tion involving the first three of a manipulator's joints (base, shoulder, and 
elbow) may be made, providing an approximation of free space suitable for rough 
arm positioning. This approach alone is viable in applications involving man- 
ipulators with three degrees of freedom or less, or those that do not require 
accurate arm positioning. 

A combination joint space/ linear programming approach would be viable in 
applications requiring accurate positioning of manipulators with more than 
three joints. Using this approach, the low-level linear optimization planner 
discussed would be coupled with a high-level planner that initially has no in- 
formation regarding the physical environment. It would incrementally "learn" 
a joint space approximation of the environment as the low-level planner moved 
about and detected obstacles. This amounts to "feeling around" blindly in the 
physical world, with the advantage of remembering every position. In a clut- 
tered environment, this system would initially find awkward paths. However, as 
the manipulator moved about more and more, the high-level planner would develop 
an accurate representation of the joint space that would be used to guide the 
arm along coll is ion- free paths. This combination approach provides a means of 
modeling the joint space that avoids the complexity of a priori calculation. 
This adaptive, hierarchical path-planning algorithm could also accommodate 
chang ing env i r onmen t s . 

The incremental, constrained-motion method . - This planning method is very 
promising, especially when coupled with an intelligent high-level planner that 
generates subgoals for the motion. No restriction as to the size of the envir- 
onment or the number of components it may contain is imposed, nor is the system 
restricted to finding paths of a certain type. The number of degrees of free- 
dom of arm motions is not limited and multiple arm systems can be accommodated. 
Sensor information can readily be incorporated in the description of the system 
and environment. Thus, this planner is especially well-suited for operation in 
situations where knowledge regarding the free space in an environment is either 
too costly to calculate or is impossible to determine ahead of time. This ap- 
plies in either extremely cluttered static worlds or in dynamic environments. 

Directions for further study . - Complex manipulator path planning in a 
static environment may be successfully accomplished using any of the three 
methods previously discussed. However, in any real world application of ro- 
botics technology, a manipulator will be expected to attain very accurate posi- 
tions many times during a given task (e.g., inspection, assembly, or machine 
operation). Time constraints may also be imposed on the operation of the en- 
tire robotic system. These two characteristics require that a path planner 
generate plans in a very short amount of time. Some marriage between existing 
methods will be necessary to accomplish this. For example, the generalized 
cone method is capable of fast rough arm positioning in a simplified represen- 
tation of an environment. The incremental, constrained-motion planner is cap- 
able of fine positioning in complex environments. Coupled with the adaptive 
learning of joint space, such a system could operate very well, especially in 
static environments. The system could switch back and forth between the two 
planning methods, using the generalized cone approach for large moves through 
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uncluttered regions of the environment and the bilevel incremental planning 
system for smaller moves requiring finer positioning. The two operations could 
be done in parallel so by the time a plan to attain a rough configuration was 
completed, a path from there to the actual goal configuration would also have 
been generated. Such a planning system is being developed at Martin Marietta 
Denver Aerospace. 

The main assumption of all present path-planning methods has been that the 
environment to be operated on does not change. For most real-world applica- 
tions, this is an invalid assumption. Many applications, such as assembly, re- 
quire the manipulator to move objects around in the environment. This is known 
as a dynamic application. The path planner may be constructed under the safe 
assumption that the system will be aware of all objects in its environment. 

The only constraint relaxed is that the obstacles remain stationary.* 

This class of path-planning problems becomes even more complex when ob- 
jects other than the manipulator may be in motion. For example, several 
manipulators may be working together in intersecting workspaces. The only 
technique suggested thus far for path planning in this situation is to allow 
only one arm to be in motion at a time whenever one is inside the working en- 
velope of another. This amounts to assuming a static configuration for one (or 
more) arms while generating a path for another and forcing the former to assume 
that configuration and stop while the latter moves. One advantage of the in- 
cremental, constrained-motion method is that it can readily be extended to ac- 
commodate interacting arms, simultaneously controlling the manipulators to 
avoid each other. 

One direction for path-planning research that would allow for less con- 
strained operation in the case just described is toward execution of partial 
plans. This method is also viable in simpler applications with time-critical 
planning constraints. Several issues to be resolved include when to begin exe- 
cution, physical "backtracking" of the manipulator when the current route is 
found to be impossible, and communication among path planners for the various 
moving bodies . 


* There are two subclasses of this type of environment, one in which objects 
are moved only by the manipulator, and one in which objects may be moved by 
other means. 
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IMAGE PROCESSING AND VIDEO SIMULATION 


A video simulation software module was developed to simulate the output of 
a video camera mounted on or near a simulated manipulator. A ROBSIM graphics 
representation depicting a scene of a PUMA 600 manipulator in a peg-in-the-hole 
scenario was generated with the ROBSIM program. Conversion of these data to a 
form used by the MOVIE. BYU package available on the Ramtek raster graphics de- 
vice was performed. The single scene was then displayed using shaded graphics 
and color with the MOVIE. BYU package to generate a raster image of the scene; 
this image was then used as input to various edge detection algorithm modules. 
This method of ROBSIM geometry database conversion to MOVIE. BYU format may be 
used to generate a raster image of any ROBSIM scene on which new image process- 
ing algorithms, such as the edge detection and thresholding operations ex- 
plained here, may be performed to enhance the resulting picture. 


Edge Detection 


An edge in a digital image is a change in intensities. However, an image 
contains many intensity changes, not all of which represent physical edges. 

For example, consider an image of a cube illuminated by a nearby point source- 
intensity changes in the image occur at the intersection of two faces because 
of the change in the angle of reflection, and along each face because of the 
change in distance from the light source. In this example, a computer vision 
system must be able to distinguish the intensity changes caused by physical 
edges from those caused by other factors. 

Edge detection schemes generally involve three stages of processing: 

1) Edge pixels are detected; 

2) Edge pixels are thinned to eliminate all but significant edges; 

3) Edge points are linked to form lines, curves, etc. 

Each of these stages is discussed in the following sections. 

Detecting edge pixels . - Consider an image as a three-dimensional surface 
where each intensity value denotes the height of the surface at that point. 
Then changes in intensity represent changes in surface height, and the magni- 
tude of an intensity change represents the magnitude of the surface slope. 
Hence, one approach to identifying the "edgeness" of a pixel is to treat the 
image as a surface and compute the magnitude of the surface slope at each 
pixel. On a continuous surface, this computation would involve computing the 
directional derivatives at each point. Because a digital image is a discrete 
function, difference operators are used to approximate the derivatives. 
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One of the first efforts at edge detection using difference operators was 
presented in [Roberts 1965]. Roberts computed an approximation to the magni- 
tude of the surface gradient at each pixel by computing first differences (dis- 
crete first derivatives) in a 2x2 neighborhood of the pixel. Although the "Ro- 
berts" operator requires relatively few computations and produces sharp edges, 
it is sensitive to noise [Peli 1982], Furthermore, the Roberts operator is di- 
rectionally sensitive, having the greatest response to edges oriented at 45 and 
135° and can only detect edges that lie to the right or below a given pixel. 

The Roberts operator is an example of what is known as a 2x2 convolution 
operator. Let f(x,y) be the gray-level values at pixel (x,y) of an image. 

Then, the Roberts operator can be expressed as 

R(x,y) = f (x,y) - f(x+l, y+1) + f(x+l, y) - f(x, y+1). 

This is equivalent to convolving the image with the two templates 



and taking the sum of the response magnitudes for the output at the pixel in 
the upper left corner. Thus the Roberts operator only takes differences to 
the right and below a given pixel and only along directions of 45 and 135° 
from the horizontal. Two more templates could be used to take differences at 
0 and 90°. A larger operator must be used to take differences in a symmetrical 
neighborhood around a pixel. The smallest symmetrical operator is a 3x3 oper- 
ator; such an operator can take differences at 0, 45, 90, and 135°. Increasing 
the size of the operators increases the number of directions along which dif- 
ferences can be taken. For example, a 5x5 operator can take differences at 0, 
30, 60, 9Q, 120, and 150°. Because increasing the size of the operators also 
increases the number of computations that must be performed, a computer vision 
system designer must consider a tradeoff between directional sensitivity and 
computational cost for this kind of operator. 

Instead of approximating the first derivative at a pixel by taking first 
differences, edges can also be detected by taking differences of average gray 
levels of adjacent neighborhoods [Rosenfeld 1982], Such operators are less 
sensitive to noise than first difference operators but tend to produce wider, 
more blurred responses. 

Second difference (discrete second derivative) operators can be used to 
detect edges. A commonly used second difference operator is the discrete 
Laplacian, which is relatively orientation-independent [Rosenfeld 1982]. Be- 
cause difference operators respond to changes in the rate of change of inten- 
sities, in a region in which the image intensities vary smoothly, the discrete 
Laplacian gives no response. At the boundaries of smooth regions, however, 
the Laplacian responds twice — once on either side of the boundary, with re- 
sponses of opposite sign. Edges are detected by locating all changes in sign, 
or zero crossings, of the response. 
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[Binford 1981] noted that a distinction should be made between the pro- 
cesses of detecting edges and localizing edges. A first difference operator 
performs well at detecting edges but responds over a broad region and is there- 
fore poor for localizing edges. A second difference operator, however, per- 
forms well at the localization task but is more sensitive to noise [Rosenfeld 
1982] . [MacVicar-Whelan 1981] reports edge localization to subpixel accuracy 
using a second difference operator. 

[Prewitt 1970] first introduced a method for detecting edges by using the 
gradient of the best-fit polynomial surface in the neighborhood of a pixel. 
Using a least squares criterion, a polynomial surface is fit to a symmetric 
neighborhood around each pixel in an image. The magnitude of the gradient of 
the best-fit surface is used as the measure of the edgeness of each pixel. The 
size of the templates used is determined by the order of the polynomial surface 
to be fit. For example, a 3x3 Prewitt operator computes the gradient magnitude 
of the best-fit plane at each pixel, whereas a 5x5 operator uses the best-fit 
quadratic surface. 

Edge detection can also be performed by fitting an ideal step edge to the 
neighborhood of a point. Generally, this technique is implemented using basis 
functions. The neighborhood of a pixel is expanded in terms of some orthogonal 
basis, and the coefficients of the expansion are compared with those for the 
expansion of an ideal step edge. By adjusting the coefficients of the ideal 
edge to minimize the squared differences between the sets of coefficients, the 
best-fit step edge through each pixel can be determined. [Hueckel 1971] was 
the first to use this technique by expanding regions in terms of the Fourier 
basis functions. [O'Gorman 1978] used Walsh functions. [Hummel 1977] used the 
Karhunen-Loeve expansion to derive a set of basis functions that are optimal 
for the detection of step edges. [Morgenthaler 1981] combined the techniques 
of surface fitting and step edge fitting to propose an edge detector using a 
local model of a step edge superimposed on a polynomial surface. 

Thinning edge pixels . - The fact was pointed out earlier that one of the 
difficulties in edge detection is detecting only the significant edges out of 
all the edges present in an image. This section discusses some of the ap- 
proaches that have been developed for thinning out the edges in an attempt to 
leave only the significant edges. 

Decision-theoretic concepts can be used to thin edges [Rosenfeld 1982]). 
Suppose that a finite number of regions can be formed in an image, and suppose 
that the probabilities of a pixel being in each region or on a bor- 
der are known. Then, if the probability densities of the edge detector re- 
sponses in the regions or borders can be estimated, the probability of a given 
pixel being an edge can be computed. 

The magnitude of the response of a first difference operator is propor- 
tional to the size of the intensity change at a point. If the assumption is 
made that large intensity changes are significant, one approach to thinning 
first difference operators is to threshold the edge picture at a particular 
edge strength, retaining only the edge pixels at which edge strength exceeds 
the threshold. However, this thinning technique fails if the strength of sig- 
nificant edges varies greatly over an image. For example, consider an image of 
objects in partial shadow. Object edges in shadow are faint and are likely to 
be lost if a global threshold is used. [Rosenfeld 1971] suggested suppressing 
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all edges except the strongest for some distance taken perpendicular to the 
edge. [Hanson 1980] used local thresholding based on the distribution of edge 
strengths in square local neighborhoods. 


[Rosenfeld 1971] also proposed a multistep technique for determining the 
"best" edge at a point. The technique requires taking the differences of aver- 
ages using neighborhoods of many different sizes. Each of the resulting edge 
pictures is thinned by suppressing nonmaxiraa across the edge. Finally, a com- 
posite edge picture is obtained by comparing the different edge strengths ob- 
tained at each pixel. Let e^k) (i,j) be the edge strength (after initial 
thinning) at pixel (i,j) using a difference of averages operator of size hxh. 
The final edge strength at (i,j) is given by (i,j) where m is the lar- 

gest value such that 


(tirt-k) , . . . 

. . .e (i,j) 


(m+k-1) , . 

< e K (i ,3) 




but 




(m-1) 


(i,j) 


The symbol "< " is taken to mean "much smaller than," and in the initial im- 
plementation of Rosenfeld and Thurston, meant roughly "less than three-quarters 
of." This algorithm is capable of detecting texture edges and was shown in 
[Peli 1982] to be fairly tolerant of noise. 

Smoothing an image before applying an edge detector can reduce the effects 
of noise. Many kinds of smoothing operators exist and are discussed in texts 
on image processing (see, for example, [Rosenfeld 1982]). Operators can be de- 
signed that combine the operations of smoothing and edge detection. 

[Binford 1981] proposed that a particular technique be implemented to 
smooth a picture for edge detection. He calls the technique "lateral inhibi- 
tion." Implementation consists of subtracting from each pixel value the aver- 
age intensity of the eight pixels immediately surrounding it. The lateral in- 
hibition operation effectively suppresses the effects of smooth variations in 
intensity. 

[Shanmugan 1979] proposed a frequency domain filter for edge detection. 

The proposed filter yields a maximum response in the vicinity of an edge. For 
the special case of a step edge, the optimal frequency domain filter is the 
second derivative of a Gaussian. Marr and Hildreth [Marr 1980] proposed an 
edge detector using the convolution of a. Gaussian smoothing function with a 
Laplacian. The Marr-Hildreth edge detector employs operators of several dif- 
ferent sizes to detect edges at various resolutions [Hildreth 1982]. 
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The Marr-Hildreth edge detector produces good results but is computa- 
tionally expensive; the templates used are huge by conventional standards — the 
smallest is 32x32 pixels [Brady 1982]. However, improvements in computer 
hardware technology may make even such large operators feasible for use in 
real-time applications. Nishihara and Larson designed a special-purpose pro- 
cessor for performing convolutions [Nishihara 1981]. The processor is capable 
of convolving a 1000x1000 image with the smallest Marr-Hildreth operator in 
about 1 second [Hildreth 1982]. Even greater speed can be expected as the 
state of the art in hardware improves. 

Linking edge pixels. - After detection and thinning of edges have been 
performed, the next step is to link edges together to impose some order on the 
image. The ways in which edge pixels are linked depend on the kinds of linear 
features expected to be in an image. 

Rosenfeld and Thurston [Rosenfeld 1971] proposed algorithms for detecting 
thin curves and wide streaks. Using their formulation, a point lies on a thin 
curve if it satisfies two conditions: 

"Condition 1: It has a pair of lower valued neighbors on opposite sides of 

it (in the direction across the curve). 

Condition 2: It has two other neighbors (in the direction along the curve 

that satisfy Condition 1." 

Variations of this algorithm can be used to link pixels belonging to a variety 
of linear features. 

[Shirai 1978] used gradient magnitude and direction, as well as a classi- 
fication of edge type to link edges. The edge types used are shown in Figure 
58. Linking proceeds in two stages. First, edge "kernels" or sets of pixels 
of the same type and direction, are identified. Then, each kernel is extended 
by tracking as far as possible in both directions. Tracking consists of pre- 
dicting the position and gradient values of the next point, and adding pixels 
to the edge if their values differ from those predicted within prespecified 
tolerances. Predictions are updated every time a pixel is added, enabling the 
techniques to track curved edges as well as straight ones. 

Ballard and Brown [Ballard 1982] advocated the use of the Hough transform 
to detect arbitrary curves. As an explanation of the technique, consider the 
example of detecting straight lines (following the development of Ballard and 
Brown). Let (x,y) be an edge pixel in an image. Any line through (x,y) is 
given by the equation y = mx + c. Because the set of all possible lines 
through (x,y) is represented by a line in m,c-space, each edge pixel in an im- 
age can be associated by a line in m,c-space; the set of edge pixels then cor- 
responds to a set of lines in m,c-space. If two edge pixels lie on the same 
line in an image, their corresponding lines will intersect in m,c-space. 
Therefore, a straight line in an image will be represented as the intersection 
of many lines in m,c-space. By detecting clusters in m,c-space, lines in an 
image can be detected. The Hough transform technique can be extended to detect 
any kind of parameterized feature. 
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Figure 58. - Classification of edge types (after [Shirai 1978]). 

Because edge detectors cannot be expected to give perfect results, gaps 
can be expected in linear features. The Hough transform can be used to detect 
features and fill in missing pixels. Another technique is to track features 
and fill in pixels that comprise gaps of a certain maximum size [Rosenfeld 
1982]. 

- s** 

The following pictures of the PUMA manipulator [Fig.; 59(a) thru ( h) ] were 
taken from a Ramtek graphics terminal after conversion of an Evans and Suther- 
land line graphics display to a solid figure display. 
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SIMULATION VALIDATION 


This section describes validation of the simulation developed under this 
contract. Two hardware systems were used to validate the ROBSIM software — a 
Martin Marietta two-degree-of-freedom planar arm and a six-axis PUMA robot. 

The sequence of validation tasks was: 

1) Identify mass and motor parameters for the planar arm; 

2) Model the planar arm in ROBSIM; 

3) Employ the same voltages used to drive the planar arm motors to drive 
the ROBSIM response simulation and compare the planar arm positions 
with the simulated position as functions of time; 

4) Repeat steps 1) through 3) for the PUMA robot. 


Planar Arm Parameter Identification 


The first step in simulating existing hardware was to determine the physi- 
cal parameters that identify the system and its motion. Mass properties of the 
planar arm were first determined by taking the arm apart to find the lengths 
and weights of the arm components. Link centers of mass and inertias were then 
calculated from these measurements. Figure 60 shows the configuration of the 
planar arm used. It included a base, shoulder motor and gears, shoulder link, 
elbow motor and gears, elbow link, and bracket. 
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Table V gives the values obtained for link masses, centers of mass, and 
y-axis inertias about the link's center of mass. 

TABLE V. - PLANAR ARM LINK MASS PROPERTIES 




Center of 

Io? inertia. 


Mass, kg 

mass , m 

kg-ra^ . 

Shoulder 

1.0297 

0.1605 

0.0132 

Elbow 

0.888 

0.2095 

0.0106 

(inc luding 
bracket) 





Motor parameters were obtained primarily from manufacturer's data sheets. 
However, the parameters that could be were tested. These included the torque 
constant, motor resistance, back-EMF, and static, coulomb, and viscous fric- 
tions. The torque constant test measured the force at a known moment arm while 
monitoring the motor current and voltage. Taking into account the amplifier 
gain allowed the torque constant to be calculated. Motor resistance was deter- 
mined from the slope of the voltage vs current curve obtained in the torque- 
constant test. Back-EMF is the open-circuit voltage produced when driving the 
motor manually through the gearing. The shaft velocity was monitored and the 
EMF constant was found from the slope of the voltage vs velocity curve. Static 
friction, which is the torque necessary to begin joint rotation, was measured 
by increasing the voltage slowly up to the point at which joint rotation begins 
and noting this voltage. Coulomb friction, which is the torque necessary to 
keep the motor rotating at a slow speed, was determined by noting the lowest 
voltage that would keep the motor turning when given a small initial velocity. 
The actual static and coulomb friction torques were calculated from the mea- 
sured voltages, amplifier gains and motor torque constants. Viscous friction, 
which is proportional to the motor angular velocity, was determined by giving 
the arm an initial velocity and then recording the angular velocity and dis- 
placement with an open circuit at the motor leads. Curve fitting these empiri- 
cal data led to extraction of the coulomb and viscous frictions. 

Before the validation was conducted, a decision was made to go to current 
control of the planar arm motors instead of voltage control. For purposes of 
the simulation, this takes the motor resistance, back-EMF, and inductance out 
of the control loop and they are set to 1, 0, and 0 respectively. Table VI 
lists the motor parameters used in the ROBSIM simulation at the beginning of 
the validation runs. 
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TABLE VI. - MOTOR PARAMETERS INITIALLY USED IN SIMULATION 


■ 

Shoulder 

Elbow 

Motor torque constant, Nm/arap 

3.58 

28.5 

Motor gear ratio 

20.25 

86.4 

Amplifier gain, amps/ volts 

0.5202 

0.2543 

Back-EMF constant, V/rad/s 

0 

0 

Motor effective inertia, kg-m^ 

0.0893 

0.6763 

Motor resistance, ohms 

1 

1 

Motor inductance, henries 

0 

0 

Coulomb friction coefficient, N-m 

0.439 

1.36 

Static friction coefficient, N-m 

0.616 

2.04 

Viscous damping coefficient, 
N-m/ rad/ s 

0.0878 

1.135 


Simulation Comparison - Elbow 


The comparison between hardware and software first looked at each joint 
separately and then at combined motion of both the shoulder and the elbow. The 
planar arm runs used were: 

1) Elbow sinusoidal motion, +20° amplitude, 0.15-Hz frequency, 0.03-s 
sampling time; 

2) Elbow trapezoidal motion, +20° amplitude, 0.15-Hz frequency, 0.03-s 
sampling time; 

3) Shoulder sinusoidal motion, +30° amplitude, 0.15-Hz frequency, 0.03-s 
sampling time; 

4) Combined shoulder and elbow motion, ;+30° amplitude, 0.1-Hz frequency, 
0.03-s sampling time. 
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The simulation modeled the planar arm using the parameters in Tables V and 
VI. Response simulation runs were executed in ROBSIM using the same control 
voltage used to control the planar arm. To compare the motion of the simula- 
tion with that of the actual hardware arm, one of the ROBSIM postprocessing op- 
tions was used. This option plays back the motion that occurred during an 
analysis run, and also simultaneously displays a stick figure whose motions re- 
present the motion of the hardware arm. 

The first hardware motion to be simulated was the sinusoidal elbow motion. 
Although the first attempt showed agreement in the direction of motion, the 
simulation amplitude and accelerations were too large. To attempt to obtain 
better agreement, the elbow motor torque constant and friction coefficients 
were reevaluated. The torque constant was determined by attaching the force/ 
torque wrist to the arm and then increasing the voltage to the elbow motor and 
measuring the force exerted against a constraint. The slope of the force volt- 
age curve multiplied by the amplifier gain and moment arm defined the motor 
torque constant. This testing obtained a torque constant of 12.8 Nra/ amp for 
the elbow motor. The static friction torque was obtained by increasing the 
voltage to the elbow joint until the joint moved. This voltage was then multi- 
plied by the amplifier gain and motor torque constant. The coulomb friction 
torque was evaluated by changing the voltage to find the lowest voltage that 
would keep the joint moving after it was given a small initial velocity. Mul- 
tiplying by the amplifier gain and torque constant gave the actual friction 
torque value. This static friction torque was also evaluated by looking at the 
planar arm motor voltage and position plots. The voltage at which motion be- 
gins (at extreme reach of each cycle) was used to calculate the static friction 
torque. The coulomb friction torque is approximately two-thirds of the static 
torque. Testing the arm gave values of 1.5 Nra and 1.0 Nra for static and cou- 
lomb friction respectively. The voltage taken from the plot indicated static 
friction to be about 2.4 Nm. In testing both sets of values in the simulation, 
it was noted that the higher friction values seemed to give better agreement 
with the hardware positions. At this point the agreement between the hardware 
and the simulation was quite good. Both reached the same amplitude and would 
start motion in the new direction at the same time. However, the simulation 
would move out more quickly and reach the end of the motion and stop before the 
hardware would. This seemed to indicate that the effective inertia of the 
joint/ link pair was too low. Because it was assumed that the link inertia was 
accurately known, it was assumed that the motor effective inertia was in error. 
The elbow motor inertia was increased until the motion of the hardware and 
simulation matched well. This meant increasing the inertia from 0.68 to 1.8. 
The positions of the elbow of the planar arm and its simulation are shown in 
Figure 61. (In the figure h denotes the hardware motion and s the simulation.) 
The negative shift of the simulation was because the arm was probably not ex- 
actly in the horizontal plane so gravity caused the motor drive voltage to be 
slightly greater in one direction than the other. 

After the sinusoidal motion was matched, the voltages used for trapezoidal 
motion of the planar arm elbow were used to control a response simulation run 
of ROBSIM. Figure 62 shows the hardware and simulation elbow positions for 
trapezoidal motion. 
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SIMULATION VALIDATION 



SIMULATION VALIDATION 



Figure 62. - Elbow simulation and hardware positions, trapezoidal motion 
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Simulation Comparison - Shoulder 


The planar arm shoulder run commanded the joint to move 30° in both posi- 
tive and negative directions. As with the elbow, simulation of the shoulder 
motion initially showed only minimal agreement with the actual hardware motion. 
The motor torque constant and the static and coulomb frictions were evaluated 
in the same manner as described for the elbow. Testing the planar arm gave a 
value of 2.6 Nra/ amp for the shoulder motor torque constant and values of 0.95 
Nm and approximately 0.88 Nm for the static and coulomb frictions respectively. 
The preceding friction torque values are for motion in the positive direction. 
Testing of the planar arm showed that frictions in the positive direction were 
much greater than in the negative direction. This was also evident by looking 
at the plot of shoulder motor voltages as a function of time. To compensate 
for the effects of the direction dependency of the friction torques, voltages 
corresponding to negative joint velocities were modified by subtracting an off- 
set voltage. This value was determined by calculating the net positive offset 
per time step from the voltage data and subtracting twice this value. The 0.2 
volt subtracted seemed to overcompensate for the problem. Trial and error 
seemed to indicate that approximately -0.18 volt was the best offset. In addi- 
tion to offsetting the negative velocity voltages, the beginning of the voltage 
file up to the first zero velocity point was cut off to prevent any initial 
conditions from affecting the simulation run. After the modifications men- 
tioned (motor torque constant, static and coulomb friction torques, and motor 
voltages), reasonable agreement was obtained between the planar arm shoulder 
motion and the simulation. But, as with the elbow, the simulation motion 
seemed to indicate a low effective inertia for the shoulder joint/ link pair. 
Again it was assumed that the link inertia was fairly accurate and that the 
joint effective inertia needed to be modified. An inertia value of 0.45 kg-m^ 
and an updated coulomb friction torque of 0.77 Nm produced good agreement be- 
tween the simulation and the hardware it was modeling. Figure 63 shows both 
the hardware and simulation positions as function of time for the sh oulder. 

SIMULRTIDN VRLIDRTION 





Table VII lists the motor parameters for the planar arm at this point. 
TABLE VII. - PLANAR ARM MOTOR PARAMETERS. 



Shoulder 

Elbow 

Motor torque constant, Nm/amp 

2.6 

12.8 

Motor gear ratio 

20.25 

86.4 

Amplifier gain 

0.5202 

0.2543 

Back-EMF constant, V/rad/s 

0 

0 

Motor effective inertia, kg-m^ 

0.45 

' 1.8 

Motor resistance, ohms 

1.0 

1.0 

Motor inductance, henries 

0 

0 

Coulomb friction coefficient, N-m 

0.77 

1.5 

Static friction coefficient, N-m 

' 0.95 

2.4 

Viscous damping coefficient, 
N-m/ rad/ s 

0.0878 

1.135 


Simulation Comparison - Combined Motion 


Thus far, good agreement between the hardware and simulation has been 
shown for each joint (shoulder and elbow) separately. To further check the 
validity of the simulation, a planar arm test case that combined the motion of 
both the shoulder and the elbow was run. Both joints were commanded to move 
30° in both the positive and negative directions at a 0.1-Hz frequency. The 
simulation was run using the motor and link parameters set previously when each 
joint was looked at separately. Figures 64 and 65 show the shoulder and elbow 
hardware and simulation positions. 


143 



COMBINED MOTION 



: Figure 64. - Hardware and simulation shoulder positions for combined motion 



Figure 65. - Hardware and simulation elbow positions for combined motion. 


As can be seen in Figure 64, the simulated shoulder position was forced to 
the positive limit because of the direction dependency of the shoulder fric- 
tions. The shoulder motor voltages corresponding to negative joint velocities 
were then offset by -0.18 (this value was determined earlier). The plot of 
shoulder hardware and software positions after offsetting the drive voltages is 
shown in Figure 66 . 

COMBINED MOTION 



Figure 66. = Shoulder hardware and simulation positions after offset. 


Figure 66 indicates that the effective inertia is too low and that the el- 
bow link inertia must be increased. (Increasing the shoulder joint or link 
inertia would change its response when run alone.) To keep the motion of the 
elbow the same when run by itself, the elbow motor inertia would have to be de- 
creased to keep the effective inertia of the elbow joint/link pair the same. As 
a first attempt, the elbow motor effective inertia was decreased to 1.0 kg-m^ 
and the link inertia (about its center of mass) was increased to 0.81 kg-m^. 
Although these values do not give the same total effective inertia as previous- 
ly noted, they were used to see if this was a valid method for correcting the 
simulation model. Figures 67 and 68 show that this is the correct method for 
updating the simulation model, and that trial and error testing would yield an 
accurate model. 
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SEC 

Figure 68. - Elbow hardware and simulation positions, combined motion. 
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PUMA Parameter Identification 


Initial experiments were performed to determine the kinematic and dynamic 
parameters for one of the two PUMA robots at NASA Langley Research Center. 

[Paul 1983] presented kinematic and dynamic parameter estimates for the PUMA 
six-degree-of-freedom manipulator. [Lee 1984] determined the kinematic para- 
meters of this arm more precisely. The evaluation described here was performed 
to validate these earlier results and to verify the consistency of parameters 
between devices of the same model. 

Figure 69 illustrates the PUMA robot and the kinematic parameters to be 
identified. The relative angles between consecutive joint axes were all as- 
sumed to be 0° or 90° and no attempt was made to verify these values. To de- 
termine the length parameters, a point on each joint axis had to be located 
first. This task was performed by marking the position (in the world coordi- 
nates) of one point on a subsequent link. The joint was then rotated by 180° 
and the new position of the point determined. (Protractors and plumb bobs were 
mounted on some of the joints to verify that 180° rotations were achieved.) 

The midway point between these two positions locates a point on the joint axis 
(Fig. 70). Using this procedure, the kinematic parameters listed in Table VIII 
were obtained (see also [Paul 1983] . Table IX shows that these agree well with 
the valued obtained by [Lee 1984] and [Paul 1983] . 



\ ' ’ ' 

\ Figure 69. - Kinematic parameters of the PUMA. 
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Figure 70. - Locating a point on the joint axis 


TABLE VIII. - PUMA KINEMATICS 



a, deg 

S 

a, mm 

-90 

0 

0 

0 

S 2 = 150.4 

L 2 = 431.0 

90 

0 

d 4 = 20.2 

-90 

L 3 = 433.1 

0 

90 

0 

0 

0- 

S 6 = 56.1 

0 


TABLE IX. - COMPARISON OF PUMA CHARACTERISTICS 


Parameter 



Measured Value* 

[Lee 1984] 

[Paul 1983] 

150.4 

149.09 

125 

431.0 

431.8 

432 

433.1 

433.07 

432 

20.2 

20.32 

19 

56.1 

5625 

- 


* All parameters in millimeters. 


















Based on the measurement technique, the accuracy of these values is esti- 
mated to be on the order of jK).5mm. Furthermore, many of the kinematic para- 
meters have assumed values. Verifying these parameters requires a more elabor- 
ate experimentation and data reduction scheme such as that proposed in [Barker 
1983]. We believe that high-accuracy positioning of robotic devices will re- 
quire some type of end-effector position sensing or online calibration because 
of these kinematic uncertainties (as well as the fact that structural deforma- 
tions occur under load.) 

A computer simulation of the system response requires several associated 
dynamic parameters to be evaluated for the system. The dynamic parameters 
needed for the simulation include masses, inertias, friction forces, etc. They 
can be broken down into two general classes: 

1) Parameters that can be identified by static force measurements; 


2) Those that require dynamic response measurements. 

First, static force measurements were performed to determine the link masses, 
link centroid locations, and friction torques at each joint. The measurements 
were performed by setting the robot up in several configurations, freeing one 
joint while leaving the relative motions at the other joints fixed and measur- 
ing the forces needed to start the arm in motion. For simplicity it was as- 
sumed that the centroid of each link lies at an unknown distance a^ along the 
link axis. The main concern was to identify the mass m£ and centroid loca- 
tion a^ for links 2 and 3 (Fig. 71) and the static friction at all joints. 

To accomplish this, force measurements were taken for eight arm configurations. 
Figure 72 schematically illustrates the eight test configurations. In each 
configuration i, the minimum applied force Fa >m in and maximum applied 
force F, a max to allow the free joint to just begin to move were measured. 

Each measurement was taken five times and Table X lists the average value and 
standard deviation for each of these measurements. The static friction for 
configuration i, Fj, can be approximated by 


F^ = F 1 - F 1 

f a , max a , mm 


and the support force Fg needed to counteract gravity is the average ap- 
plied force 


= p^ + pi 

g a, max a, min 
2 
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TABLE X. - MEASURED FRICTION AND GRAVITY FORCES 


i 

^ajinin/a * 

pi < 

* a , max / a 

F i 


a 

-35.6/0.49 

39.4/0.55 

- 

37 

.5 

h 

51.2/089 

66.7/0.53 

58.9 

7. 

78 

c 

13.4/0.12 

26.6/0.47 

20.0 

6. 

58 

d 

80.1/0.67 

112.5/0.84 

96.3 

16 

.2 

e 

38.7/1.07 

64.9/0.84 

51.8 

13 

.1 

f 

11.8/0.71 

23.1/0.31 

17.5 

5. 

65 

g 

10.0/0.22 

24.7/0.18 

17.4 

7. 

34 

h 

12.0/0.09 

22.6/0.11 

17.3 

5. 

32 

* a - standard deviation; all values given in newtons. 


Based oh the results listed in Table X and the effective moment arms for 
the different configurations shown in Figure 72, the joint friction forces can 
be evaluated. Table XI lists the resulting joint friction torques. Note that 
the configurations b, d, and e all give values for the static friction at joint 
2. The values from configurations b and d are 6.72 and 6.82 Nm, but the mea- 
sured value from configuration e is 8.82 Nm which is inconsistent with the 
other results. Unfortunately, the test could not be readily repeated, so it is 
unclear whether this indicates a true anomaly or that an error was made in re- 
cording the arm configuration. The latter explanation seems unlikely because 
the gravity force in this configuration agrees closely with the results for 
configuration b, as discussed later. 
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TABLE XI. - JOINT STATIC FRICTION TORQUES 


Joint 

Static Friction, Nm 

1 

18.8 

2 

6.8 

3 

3.3 

4 

0.84 

5 

0.84 

6 

1.09 


Analysis of the results in this and the preceding section provide the 
following conclusions and suggested modifications of the testing procedure: 

1) The standard deviation for each set of measurements is quite small 
(approximately 1%), providing good confidence in the resulting values; 

2) The Fg values for measurement configurations f, g, and h agree 
closely, as they should, because the supported weight is the same in 
each of these cases; 

3) The directional nature of the friction forces must be evaluated. As 
seen from the results from joint 1 of the PUMA and from the planar 
arm, the friction coefficient for one direction of motion can signifi- 
cantly differ from the value for the other direction; 

4) The dry friction when the joint is just barely moving should be evalu- 
ated in addition to the static value determined by these tests; 

5) The friction forces from the input-driven system should be compared 
with the values obtained by the technique presented here in which the 
output is driven. 

The force measurements from configurations b, c, d, and e also provide values 
for the mass and centroid locations. 

The discussion in the adaptive control subsection of this report showed 
that only certain combinations of these terms can be identified, i.e., response 
measurements cannot differentiate between some of the terras. This also means 
that these observable mass combinations determine the dynamic properties and 
the individual components need not be evaluated explicitly. The disassembled 
manipulator must be measured or engineering approximations must be used to ex- 
plicitly evaluate the individual terras. In the measurement ( cases considered 
here, the terms that can be identified are D]^ = 111333 and D14 = 
m 2 a 2 + m 3^ J 2* 
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For example, configuration c provides the result (Fig. 71 and 72) 
m 3 a 3 g = F a d = (20. ON) (0.497m) 

where g = 9.8 m/s^ is the acceleration caused by gravity. This leads to 
D 15 = m 3 a 3 = 1.01 kg-m. 

Similarly, configuration d provides the value for D 34 as 
g[m 2 a 2 + 11131 , 2 ] = F a d = (96 . 3N) (0 .421)m 
D 14 = m2 a 2 + m 3h2 = 4.14 kg-m. 

These values can be validated by comparing them with the measurements from con- 
figurations b and e. For configuration b, the values given above yield 

g[m 2 a 2 + 103(12 + 33 )] = g(D ]4 + D 15 ) = 50.5 Nm, 

which is within 1 % of the measured value 

F a d = (58.9N) (0.864m) = 50.9 Nm. 

Also, for configuration e, the identified mass properties produce 

g cos 45 °[m 2 a 2 + m 3 (L 2 + a 2 )] = 0.7071 g (D 34 = D 35 ) = 35.7 Nm, 
which differs by about 2 % from the measured value 
F a d = (51.8N)(0.673m) = 34.9 Nm. 

In addition to the static force measurements for determining the system 
parameters, dynamic response was also measured. These tests consisted of run- 
ning certain trajectories with a single joint moving at a time and recording 
the position and driving current profiles during the motion. 

The data from the dynamic response runs were transferred to Martin 
Marietta. The intent of the effort was to first plot position, velocity and 
acceleration profiles of the single-joint PUMA runs. The PUMA would then be 
modeled using ROBSIM and response simulations would be run using the same cur- 
rents as used to run the actual PUMA arm. After this, positions, velocities, 
and accelerations of the simulated motions would be plotted and compared with 
the joint motions of the actual PUMA. However, when the motion profiles for 
the actual hardware joints were plotted, a lot of noise was present. Figures 
73, 74 and 75 show position, velocity and acceleration plots for joint 3 of 
the PUMA and the noise present in the signals. At this time of this writing 
the source of the noise has not been determined and further testing is nec- 
essary for this evaluation. 
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Figure 75. - PUMA joint 3 acceleration profile. 
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CONCLUDING REMARKS 


This report has documented the work done in Phases II and III of contract 
NASI- 16759, Evaluation of Automated Decisionmaking Methodologies and Develop- 
ment of an Integrated Robotic System Simulation. The tasks defined for Phase 
II (tasks 9 thru 13) and Phase III (14 thru 20) have significantly increased 
the capabilities of the robotic simulation (ROBSIM) package and the technolo- 
gies associated with the automation and operation of advanced manipulator sys- 
tems listed: 

1) System definition - The implementation of this module allows a user to 
define a manipulator system consisting of multiple arms, load objects 
and an environment. This addressed task 12 of the contract; 

2) Analysis capabilities - This has added the capabilities of kinematic 
analysis, requirements analysis, and response simulation for investi- 
gating manipulator motion. This addressesed tasks 14, 15, and 16; 

3) Postprocessing - This module allows the user to better evaluate sys- 
tem analysis results through the graphic replay of simulated motion 
or manipulator parameter plots; 

4) Control - Various control methods were investigated and those imple- 
mented in ROBSIM include manual force/ torque and active compliance 
control. This addressed tasks 13, 17 and 19 of the contract; 

5) Trajectory planning - Three methods of obstacle avoidance were imple- 
mented and evaluated and research was conducted in the field of tra- 
jectory planning. This work addressed tasks 9 and 18; 

6) Image processing - The module implemented video simulation and edge 
detection and addressed tasks 10 and 11; 

7) Simulation validation - The software simulation was validated in con- 
junction with the Automation Technology Branch of NASA-LRC. This work 
addresses task 20 of the contract. 

Target areas for future enhancement of the ROBSIM capabilities include 
greater ease of user interaction with the program, expanded manipulator model- 
ing abilities, and the addition of system compliance in the model. 

Expansion of the ROBSIM software could now be directed toward support of 
teleoperator and robotic systems capable of such remote space operations as the 
remote orbital servicing system (ROSS) concept. The simulation could support 
dual manipulator arms attached to a carrier spacecraft in orbit. Forces, mo- 
ments and motions typical of a dual-arm robot/host vehicle system of this type 
would be included in the kinematic and dynamic solutions. It is anticipated 
that the models for the manipulator motors, the sensors and end-effector will 
warrant the development of new software modules and laboratory validation ex- 
periments. Figure 76 is a sample of a preliminary ROSS simulation graphics 
display. 
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Figure 76. - Preliminary ROSS simulation graphics display. 

An investigation of concepts for a system that allows manual designation 
of objects within a visual (television) scene and that performs high-accuracy 
position measurements could be conducted. A description of one or more systems 
that provides this capability for the ROSS configuration would be developed. 
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V 


Continuing work in the trajectory planning field would couple the man/ 
machine interface with the software-simulation of remote systems. Assessment 
of the problem of path trajectory singularity avoidance and various approaches 
to autonomous task planning should be researched. A task-oriented command mo- 
dule that incorporates task primitive commands into the simulation user inter- 
face would be documented and developed. Executing a task command will initiate 
a sequence of appropriate sensor, actuator and state commands. Techniques em- 
ploying rule-based systems and theorem provers may solve the problems associ- 
ated with task scheduling to achieve a specific mission goal. 

Validation of the existing software simulation modules will continue, and 
the models for actuators, joints and links should be extended to fit a more 
generalized set of hardware robotic systems. To effect a better representa- 
tion of the robotic hardware defined in the system development phase of the 
program, an interface to allow transfer and conversion of CAD/CAM (computer- 
aided design and manufacturing) or geometric modeling data to the ROBSIM format 
should be designed. The establishment of a compatible exchange of product de- 
finition between CAD/CAM and ROBSIM would enhance the general applicability of 
ROBS IM-gene rated databases and serve to lessen duplication of efforts in mo- 
deling hardware component systems. The current method for definition of robo- 
tic systems within ROBSIM proceeds by defining primitive robotic hardware geo- 
metry and provides only rough sketches of the actual hardware properties. 

Figure 77 depicts a CAD/CAM robot arm that was converted from a standardized 
IGES (initial graphics exchange specification) formatted file for display on a 
vector graphics terminal. 



Figure 77. - Display of robot arm converted from a CAD/CAM database. 
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Current processing capabilities in the ROBSIM program may be carried out 
on virtually any modern digital computing machine of the supermini or higher 
class. However, with the exception of the newest and fastest machines, the 
necessary speed for real-time simulation processing will not be achieved by a 
single machine. Specialized machines with multiple processor architectures 
will be necessary to accomplish this goal. Areas such as mechanical configu- 
ration optimization, accuracy analysis, control requirements, stability, and 
work environment studies require simulation but not necessarily in real time. 
Several significant long-range requirements point to the need for a real-time 
simulation capability, and research of the following available systems is re- 
commended: 

1) Realistic graphics - When the simulation system is coupled with a gra- 
phics display of the computer solutions in real time, the results are 
much more realistic; 

2) Surrogate hardware - Properly simulated hardware may be used to re- 
place hardware subsystems that have failed or are not yet available. 
The remainder of the hardware may be used in the system as intended. 
System problems may be isolated by substituting software modules for 
suspected failing hardware modules. System integration may be ap- 
proached more gradually by bringing hardware on line one module at a 
time or as the hardware is available; 

3) Real time testbed for algorithm development - Control and estimation 
algorithms may be evaluated and optimized using the true or emulated 
control computer without the use of scaling within the algorithms, and 
with no potential damage to the existing and manipulator assembly; 

4) Real-time test bed for path planning and man/ machine interface - Safe , 
real-time response to decisions and trajectory decisions as dictated 
by sensor output and command input is very important for a smooth flow 
of the man/machine interface within the simulation and to achieve a 
diversified approach to task-oriented plans. 
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